interaction_handler.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Adam Leeper */
36 
37 #ifndef MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_
38 #define MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_
39 
42 //#include <moveit/robot_interaction/robot_interaction.h>
43 #include <visualization_msgs/InteractiveMarkerFeedback.h>
45 #include <tf/tf.h>
46 
47 namespace robot_interaction
48 {
49 MOVEIT_CLASS_FORWARD(InteractionHandler);
50 MOVEIT_CLASS_FORWARD(RobotInteraction);
51 MOVEIT_CLASS_FORWARD(KinematicOptionsMap);
52 
53 class EndEffectorInteraction;
54 class JointInteraction;
56 
66 typedef boost::function<void(InteractionHandler*, bool)> InteractionHandlerCallbackFn;
67 
77 {
78 public:
79  // Use this constructor if you have an initial RobotState already.
80  InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
81  const robot_state::RobotState& initial_robot_state,
83 
84  // Use this constructor to start with a default state.
85  InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
87 
88  // DEPRECATED.
89  InteractionHandler(const std::string& name, const robot_state::RobotState& initial_robot_state,
91  // DEPRECATED.
92  InteractionHandler(const std::string& name, const robot_model::RobotModelConstPtr& model,
94 
96  {
97  }
98 
99  const std::string& getName() const
100  {
101  return name_;
102  }
103 
106 
109 
110  void setUpdateCallback(const InteractionHandlerCallbackFn& callback);
111  const InteractionHandlerCallbackFn& getUpdateCallback() const;
112  void setMeshesVisible(bool visible);
113  bool getMeshesVisible() const;
114  void setControlsVisible(bool visible);
115  bool getControlsVisible() const;
116 
120  void setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::Pose& m);
121 
125  void setPoseOffset(const JointInteraction& j, const geometry_msgs::Pose& m);
126 
131  bool getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::Pose& m);
132 
137  bool getPoseOffset(const JointInteraction& vj, geometry_msgs::Pose& m);
138 
142  void clearPoseOffset(const EndEffectorInteraction& eef);
143 
146  void clearPoseOffset(const JointInteraction& vj);
147 
149  void clearPoseOffsets();
150 
154  void setMenuHandler(const std::shared_ptr<interactive_markers::MenuHandler>& mh);
155 
159  const std::shared_ptr<interactive_markers::MenuHandler>& getMenuHandler();
160 
162  void clearMenuHandler();
163 
169  bool getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef, geometry_msgs::PoseStamped& pose);
170 
176  bool getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::PoseStamped& pose);
177 
182 
186 
189  void clearLastMarkerPoses();
190 
193  virtual void handleEndEffector(const EndEffectorInteraction& eef,
194  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
195 
198  virtual void handleJoint(const JointInteraction& vj,
199  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
200 
203  virtual void handleGeneric(const GenericInteraction& g,
204  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
205 
208  virtual bool inError(const EndEffectorInteraction& eef) const;
209 
212  virtual bool inError(const JointInteraction& vj) const;
213 
215  virtual bool inError(const GenericInteraction& g) const;
216 
219  void clearError(void);
220 
224  void setRobotInteraction(RobotInteraction* robot_interaction);
225 
226 protected:
227  bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
228  const geometry_msgs::Pose& offset, geometry_msgs::PoseStamped& tpose);
229 
230  const std::string name_;
231  const std::string planning_frame_;
233 
234 private:
235  typedef boost::function<void(InteractionHandler*)> StateChangeCallbackFn;
236 
237  // Update RobotState using a generic interaction feedback message.
238  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
239  void updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g,
240  const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback,
241  StateChangeCallbackFn* callback);
242 
243  // Update RobotState for a new pose of an eef.
244  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
245  void updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef,
246  const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback);
247 
248  // Update RobotState for a new joint position.
249  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
250  void updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* pose,
251  StateChangeCallbackFn* callback);
252 
253  // Set the error state for \e name.
254  // Returns true if the error state for \e name changed.
255  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
256  bool setErrorState(const std::string& name, bool new_error_state);
257 
260  bool getErrorState(const std::string& name) const;
261 
262  // Contains the (user-programmable) pose offset between the end-effector
263  // parent link (or a virtual joint) and the desired control frame for the
264  // interactive marker. The offset is expressed in the frame of the parent
265  // link or virtual joint. For example, on a PR2 an offset of +0.20 along
266  // the x-axis will move the center of the 6-DOF interactive marker from
267  // the wrist to the finger tips.
268  // PROTECTED BY offset_map_lock_
269  std::map<std::string, geometry_msgs::Pose> offset_map_;
270 
271  // Contains the most recent poses received from interactive marker feedback,
272  // with the offset removed (e.g. in theory, coinciding with the end-effector
273  // parent or virtual joint). This allows a user application to query for the
274  // interactive marker pose (which could be useful for robot control using
275  // gradient-based methods) even when the IK solver failed to find a valid
276  // robot state that satisfies the feedback pose.
277  // PROTECTED BY pose_map_lock_
278  std::map<std::string, geometry_msgs::PoseStamped> pose_map_;
279 
280  // The RobotInteraction we are associated with.
281  // This is never safe to use because the RobotInteraction could be deleted at
282  // any time.
283  // Therefore it is stored as a void* to discourage its use.
284  // This is only used inside setKinematicOptions() with the state_lock_ held.
285  // That function should only be called from RobotInteraction methods.
286  // PROTECTED BY state_lock_
287  const void* robot_interaction_;
288 
289  boost::mutex pose_map_lock_;
290  boost::mutex offset_map_lock_;
291 
292  // per group options for doing kinematics.
293  // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The
294  // CONTENTS is protected internally.
295  KinematicOptionsMapPtr kinematic_options_map_;
296 
297  // A set of Interactions for which the pose is invalid.
298  // PROTECTED BY state_lock_
299  std::set<std::string> error_state_;
300 
301  // For adding menus (and associated callbacks) to all the
302  // end-effector and virtual-joint interactive markers
303  //
304  // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The
305  // CONTENTS is not.
306  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_;
307 
308  // Called when the RobotState maintained by the handler changes.
309  // The caller may, for example, redraw the robot at the new state.
310  // handler is the handler that changed.
311  // error_state_changed is true if an end effector's error state may have
312  // changed.
313  //
314  // PROTECTED BY state_lock_ - the function pointer is protected, but the call
315  // is made without any lock held.
316  boost::function<void(InteractionHandler* handler, bool error_state_changed)> update_callback_;
317 
318  // PROTECTED BY state_lock_
320 
321  // PROTECTED BY state_lock_
323 
324  // remove '_' characters from name
325  static std::string fixName(std::string name);
326 
327 public:
328  // DEPRECATED FUNCTIONS.
329  // DO NOT USE THESE. Instead access the KinematicOptions by calling
330  // RobotInteraction::getKinematicOptionsMap()
331  void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn& callback);
332  void setIKTimeout(double timeout);
333  void setIKAttempts(unsigned int attempts);
336  void setKinematicsQueryOptionsForGroup(const std::string& group_name,
337  const kinematics::KinematicsQueryOptions& options);
338 };
339 }
340 
341 #endif
void clearPoseOffsets()
Clear the pose offset for all end-effectors and virtual joints.
robot_state::RobotStateConstPtr getState() const
get read-only access to the state.
void updateStateJoint(robot_state::RobotState *state, const JointInteraction *vj, const geometry_msgs::Pose *pose, StateChangeCallbackFn *callback)
boost::function< void(InteractionHandler *handler, bool error_state_changed)> update_callback_
bool getErrorState(const std::string &name) const
void clearLastMarkerPoses()
Clear the last interactive_marker command poses for all end-effectors and joints. ...
void clearMenuHandler()
Remove the menu handler for this interaction handler.
const std::string & getName() const
kinematics::KinematicsQueryOptions getKinematicsQueryOptions() const
void setRobotInteraction(RobotInteraction *robot_interaction)
This should only be called by RobotInteraction. Associates this InteractionHandler to a RobotInteract...
void setState(const robot_state::RobotState &state)
Set the state to the new value.
void clearLastJointMarkerPose(const JointInteraction &vj)
Clear the last interactive_marker command pose for the given joint.
std::map< std::string, geometry_msgs::Pose > offset_map_
bool getLastJointMarkerPose(const JointInteraction &vj, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for a joint.
void setKinematicsQueryOptionsForGroup(const std::string &group_name, const kinematics::KinematicsQueryOptions &options)
bool getLastEndEffectorMarkerPose(const EndEffectorInteraction &eef, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for an end-effector.
void setPoseOffset(const EndEffectorInteraction &eef, const geometry_msgs::Pose &m)
Set the offset from EEF to its marker.
virtual void handleJoint(const JointInteraction &vj, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Update the internal state maintained by the handler using information from the received feedback mess...
virtual void handleEndEffector(const EndEffectorInteraction &eef, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Update the internal state maintained by the handler using information from the received feedback mess...
void clearPoseOffset(const EndEffectorInteraction &eef)
Clear the interactive marker pose offset for the given end-effector.
bool getPoseOffset(const EndEffectorInteraction &eef, geometry_msgs::Pose &m)
Get the offset from EEF to its marker.
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_
static std::string fixName(std::string name)
boost::function< void(InteractionHandler *)> StateChangeCallbackFn
boost::shared_ptr< tf::Transformer > tf_
const std::shared_ptr< interactive_markers::MenuHandler > & getMenuHandler()
Get the menu handler that defines menus and callbacks for all interactive markers drawn by this inter...
virtual bool inError(const EndEffectorInteraction &eef) const
Check if the marker corresponding to this end-effector leads to an invalid state. ...
void updateStateGeneric(robot_state::RobotState *state, const GenericInteraction *g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr *feedback, StateChangeCallbackFn *callback)
boost::function< void(InteractionHandler *, bool)> InteractionHandlerCallbackFn
void clearLastEndEffectorMarkerPose(const EndEffectorInteraction &eef)
Clear the last interactive_marker command pose for the given end-effector.
bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const geometry_msgs::Pose &offset, geometry_msgs::PoseStamped &tpose)
void setKinematicsQueryOptions(const kinematics::KinematicsQueryOptions &opt)
void updateStateEndEffector(robot_state::RobotState *state, const EndEffectorInteraction *eef, const geometry_msgs::Pose *pose, StateChangeCallbackFn *callback)
const InteractionHandlerCallbackFn & getUpdateCallback() const
std::map< std::string, geometry_msgs::PoseStamped > pose_map_
void setUpdateCallback(const InteractionHandlerCallbackFn &callback)
void clearError(void)
Clear any error settings. This makes the markers appear as if the state is no longer invalid...
Maintain a RobotState in a multithreaded environment.
virtual void handleGeneric(const GenericInteraction &g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Update the internal state maintained by the handler using information from the received feedback mess...
InteractionHandler(const RobotInteractionPtr &robot_interaction, const std::string &name, const robot_state::RobotState &initial_robot_state, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >())
void setIKAttempts(unsigned int attempts)
void setMenuHandler(const std::shared_ptr< interactive_markers::MenuHandler > &mh)
Set the menu handler that defines menus and callbacks for all interactive markers drawn by this inter...
bool setErrorState(const std::string &name, bool new_error_state)
void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn &callback)
MOVEIT_CLASS_FORWARD(InteractionHandler)


robot_interaction
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:32