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 #pragma once
38 
39 #include <geometry_msgs/PoseStamped.h>
42 //#include <moveit/robot_interaction/robot_interaction.h>
43 #include <visualization_msgs/InteractiveMarkerFeedback.h>
45 #include <tf2_ros/buffer.h>
46 
47 namespace robot_interaction
48 {
49 MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc
50 MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc
51 MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc
52 
53 struct EndEffectorInteraction;
54 struct JointInteraction;
55 struct GenericInteraction;
56 
66 typedef boost::function<void(InteractionHandler*, bool)> InteractionHandlerCallbackFn;
67 
76 class InteractionHandler : public LockedRobotState
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 moveit::core::RobotState& initial_robot_state,
82  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>());
83 
84  // Use this constructor to start with a default state.
85  InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
86  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>());
87 
88  // DEPRECATED.
89  InteractionHandler(const std::string& name, const moveit::core::RobotState& initial_robot_state,
90  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>());
91  // DEPRECATED.
92  InteractionHandler(const std::string& name, const moveit::core::RobotModelConstPtr& model,
93  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>());
94 
95  ~InteractionHandler() override
96  {
97  }
98 
99  const std::string& getName() const
100  {
101  return name_;
102  }
103 
106 
109 
110  void setUpdateCallback(const InteractionHandlerCallbackFn& callback);
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();
220 
221 protected:
222  bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
223  const geometry_msgs::Pose& offset, geometry_msgs::PoseStamped& tpose);
224 
225  const std::string name_;
226  const std::string planning_frame_;
227  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
228 
229 private:
230  typedef boost::function<void(InteractionHandler*)> StateChangeCallbackFn;
231 
232  // Update RobotState using a generic interaction feedback message.
233  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
235  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
236  StateChangeCallbackFn& callback);
237 
238  // Update RobotState for a new pose of an eef.
239  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
241  const geometry_msgs::Pose& pose, StateChangeCallbackFn& callback);
242 
243  // Update RobotState for a new joint position.
244  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
245  void updateStateJoint(moveit::core::RobotState& state, const JointInteraction& vj, const geometry_msgs::Pose& pose,
246  StateChangeCallbackFn& callback);
247 
248  // Set the error state for \e name.
249  // Returns true if the error state for \e name changed.
250  // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
251  bool setErrorState(const std::string& name, bool new_error_state);
252 
255  bool getErrorState(const std::string& name) const;
256 
257  // Contains the (user-programmable) pose offset between the end-effector
258  // parent link (or a virtual joint) and the desired control frame for the
259  // interactive marker. The offset is expressed in the frame of the parent
260  // link or virtual joint. For example, on a PR2 an offset of +0.20 along
261  // the x-axis will move the center of the 6-DOF interactive marker from
262  // the wrist to the finger tips.
263  // PROTECTED BY offset_map_lock_
264  std::map<std::string, geometry_msgs::Pose> offset_map_;
265 
266  // Contains the most recent poses received from interactive marker feedback,
267  // with the offset removed (e.g. in theory, coinciding with the end-effector
268  // parent or virtual joint). This allows a user application to query for the
269  // interactive marker pose (which could be useful for robot control using
270  // gradient-based methods) even when the IK solver failed to find a valid
271  // robot state that satisfies the feedback pose.
272  // PROTECTED BY pose_map_lock_
273  std::map<std::string, geometry_msgs::PoseStamped> pose_map_;
274 
275  boost::mutex pose_map_lock_;
276  boost::mutex offset_map_lock_;
277 
278  // per group options for doing kinematics.
279  // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The
280  // CONTENTS is protected internally.
281  KinematicOptionsMapPtr kinematic_options_map_;
282 
283  // A set of Interactions for which the pose is invalid.
284  // PROTECTED BY state_lock_
285  std::set<std::string> error_state_;
286 
287  // For adding menus (and associated callbacks) to all the
288  // end-effector and virtual-joint interactive markers
289  //
290  // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The
291  // CONTENTS is not.
292  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_;
293 
294  // Called when the RobotState maintained by the handler changes.
295  // The caller may, for example, redraw the robot at the new state.
296  // handler is the handler that changed.
297  // error_state_changed is true if an end effector's error state may have
298  // changed.
299  //
300  // PROTECTED BY state_lock_ - the function pointer is protected, but the call
301  // is made without any lock held.
302  boost::function<void(InteractionHandler* handler, bool error_state_changed)> update_callback_;
303 
304  // PROTECTED BY state_lock_
306 
307  // PROTECTED BY state_lock_
309 
310  // remove '_' characters from name
311  static std::string fixName(std::string name);
312 };
313 } // namespace robot_interaction
robot_interaction::InteractionHandler::getMenuHandler
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...
Definition: interaction_handler.cpp:223
robot_interaction::InteractionHandler::menu_handler_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_
Definition: interaction_handler.h:324
robot_interaction::InteractionHandler::getPoseOffset
bool getPoseOffset(const EndEffectorInteraction &eef, geometry_msgs::Pose &m)
Get the offset from EEF to its marker.
Definition: interaction_handler.cpp:151
robot_interaction::InteractionHandler::getName
const std::string & getName() const
Definition: interaction_handler.h:131
robot_interaction::InteractionHandler::StateChangeCallbackFn
boost::function< void(InteractionHandler *)> StateChangeCallbackFn
Definition: interaction_handler.h:262
robot_interaction::InteractionHandler::transformFeedbackPose
bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const geometry_msgs::Pose &offset, geometry_msgs::PoseStamped &tpose)
Definition: interaction_handler.cpp:406
robot_interaction::GenericInteraction
Definition: interaction.h:118
robot_interaction::InteractionHandler::updateStateJoint
void updateStateJoint(moveit::core::RobotState &state, const JointInteraction &vj, const geometry_msgs::Pose &pose, StateChangeCallbackFn &callback)
Definition: interaction_handler.cpp:346
robot_interaction::InteractionHandler::clearLastEndEffectorMarkerPose
void clearLastEndEffectorMarkerPose(const EndEffectorInteraction &eef)
Clear the last interactive_marker command pose for the given end-effector.
Definition: interaction_handler.cpp:199
robot_interaction::InteractionHandler::setUpdateCallback
void setUpdateCallback(const InteractionHandlerCallbackFn &callback)
Definition: interaction_handler.cpp:441
robot_interaction::InteractionHandler::setMeshesVisible
void setMeshesVisible(bool visible)
Definition: interaction_handler.cpp:453
menu_handler.h
buffer.h
robot_interaction::InteractionHandler::pose_map_
std::map< std::string, geometry_msgs::PoseStamped > pose_map_
Definition: interaction_handler.h:305
robot_interaction::InteractionHandlerCallbackFn
boost::function< void(InteractionHandler *, bool)> InteractionHandlerCallbackFn
Definition: interaction_handler.h:87
robot_interaction::JointInteraction
Definition: interaction.h:159
moveit::core::RobotState
robot_interaction::InteractionHandler::offset_map_lock_
boost::mutex offset_map_lock_
Definition: interaction_handler.h:308
robot_interaction::InteractionHandler::handleGeneric
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...
Definition: interaction_handler.cpp:235
robot_interaction::InteractionHandler::kinematic_options_map_
KinematicOptionsMapPtr kinematic_options_map_
Definition: interaction_handler.h:313
robot_interaction::InteractionHandler::display_controls_
bool display_controls_
Definition: interaction_handler.h:340
robot_interaction::InteractionHandler::clearPoseOffsets
void clearPoseOffsets()
Clear the pose offset for all end-effectors and virtual joints.
Definition: interaction_handler.cpp:145
robot_interaction::InteractionHandler::inError
virtual bool inError(const EndEffectorInteraction &eef) const
Check if the marker corresponding to this end-effector leads to an invalid state.
Definition: interaction_handler.cpp:362
robot_interaction::InteractionHandler::setMenuHandler
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...
Definition: interaction_handler.cpp:217
robot_interaction::InteractionHandler::setErrorState
bool setErrorState(const std::string &name, bool new_error_state)
Definition: interaction_handler.cpp:385
robot_interaction::InteractionHandler::clearError
void clearError()
Clear any error settings. This makes the markers appear as if the state is no longer invalid.
Definition: interaction_handler.cpp:377
robot_interaction::InteractionHandler::name_
const std::string name_
Definition: interaction_handler.h:257
robot_interaction::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(InteractionHandler)
robot_interaction::LockedRobotState::setState
void setState(const moveit::core::RobotState &state)
Set the state to the new value.
Definition: locked_robot_state.cpp:62
robot_interaction::InteractionHandler::clearLastMarkerPoses
void clearLastMarkerPoses()
Clear the last interactive_marker command poses for all end-effectors and joints.
Definition: interaction_handler.cpp:211
robot_interaction::InteractionHandler::error_state_
std::set< std::string > error_state_
Definition: interaction_handler.h:317
robot_interaction::InteractionHandler::update_callback_
boost::function< void(InteractionHandler *handler, bool error_state_changed)> update_callback_
Definition: interaction_handler.h:334
robot_interaction::InteractionHandler::pose_map_lock_
boost::mutex pose_map_lock_
Definition: interaction_handler.h:307
locked_robot_state.h
robot_interaction::EndEffectorInteraction
Definition: interaction.h:139
robot_interaction::InteractionHandler::getUpdateCallback
const InteractionHandlerCallbackFn & getUpdateCallback() const
Definition: interaction_handler.cpp:447
robot_interaction::InteractionHandler::setControlsVisible
void setControlsVisible(bool visible)
Definition: interaction_handler.cpp:465
robot_interaction::InteractionHandler::display_meshes_
bool display_meshes_
Definition: interaction_handler.h:337
class_forward.h
robot_interaction::InteractionHandler::clearMenuHandler
void clearMenuHandler()
Remove the menu handler for this interaction handler.
Definition: interaction_handler.cpp:229
robot_interaction::InteractionHandler::clearLastJointMarkerPose
void clearLastJointMarkerPose(const JointInteraction &vj)
Clear the last interactive_marker command pose for the given joint.
Definition: interaction_handler.cpp:205
robot_interaction::InteractionHandler::~InteractionHandler
~InteractionHandler() override
Definition: interaction_handler.h:127
robot_interaction::InteractionHandler::getControlsVisible
bool getControlsVisible() const
Definition: interaction_handler.cpp:471
robot_interaction::InteractionHandler::getLastJointMarkerPose
bool getLastJointMarkerPose(const JointInteraction &vj, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for a joint.
Definition: interaction_handler.cpp:187
robot_interaction::LockedRobotState::getState
moveit::core::RobotStateConstPtr getState() const
get read-only access to the state.
Definition: locked_robot_state.cpp:56
robot_interaction
Definition: interaction.h:54
robot_interaction::InteractionHandler::handleJoint
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...
Definition: interaction_handler.cpp:284
robot_interaction::InteractionHandler::planning_frame_
const std::string planning_frame_
Definition: interaction_handler.h:258
robot_interaction::InteractionHandler::getMeshesVisible
bool getMeshesVisible() const
Definition: interaction_handler.cpp:459
robot_interaction::InteractionHandler::clearPoseOffset
void clearPoseOffset(const EndEffectorInteraction &eef)
Clear the interactive marker pose offset for the given end-effector.
Definition: interaction_handler.cpp:133
robot_interaction::InteractionHandler::updateStateGeneric
void updateStateGeneric(moveit::core::RobotState &state, const GenericInteraction &g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, StateChangeCallbackFn &callback)
Definition: interaction_handler.cpp:317
robot_interaction::InteractionHandler::offset_map_
std::map< std::string, geometry_msgs::Pose > offset_map_
Definition: interaction_handler.h:296
robot_interaction::InteractionHandler
Definition: interaction_handler.h:108
robot_interaction::InteractionHandler::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: interaction_handler.h:259
robot_interaction::InteractionHandler::getLastEndEffectorMarkerPose
bool getLastEndEffectorMarkerPose(const EndEffectorInteraction &eef, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for an end-effector.
Definition: interaction_handler.cpp:175
robot_interaction::InteractionHandler::setPoseOffset
void setPoseOffset(const EndEffectorInteraction &eef, const geometry_msgs::Pose &m)
Set the offset from EEF to its marker.
Definition: interaction_handler.cpp:121
robot_interaction::InteractionHandler::InteractionHandler
InteractionHandler(const RobotInteractionPtr &robot_interaction, const std::string &name, const moveit::core::RobotState &initial_robot_state, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >())
Definition: interaction_handler.cpp:90
robot_interaction::InteractionHandler::getErrorState
bool getErrorState(const std::string &name) const
Definition: interaction_handler.cpp:400
robot_interaction::InteractionHandler::handleEndEffector
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...
Definition: interaction_handler.cpp:252
robot_interaction::InteractionHandler::updateStateEndEffector
void updateStateEndEffector(moveit::core::RobotState &state, const EndEffectorInteraction &eef, const geometry_msgs::Pose &pose, StateChangeCallbackFn &callback)
Definition: interaction_handler.cpp:330
robot_interaction::InteractionHandler::fixName
static std::string fixName(std::string name)
Definition: interaction_handler.cpp:115


robot_interaction
Author(s): Ioan Sucan
autogenerated on Sat Mar 15 2025 02:26:54