robot_interaction.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_ROBOT_INTERACTION_
38 #define MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_
39 
40 #include <visualization_msgs/InteractiveMarkerFeedback.h>
41 #include <visualization_msgs/InteractiveMarker.h>
42 #include <geometry_msgs/PoseArray.h>
47 #include <boost/function.hpp>
48 #include <boost/thread.hpp>
49 #include <memory>
50 
51 // This is needed for legacy code that includes robot_interaction.h but not
52 // interaction_handler.h
54 
55 namespace interactive_markers
56 {
57 class InteractiveMarkerServer;
58 }
59 
60 namespace robot_interaction
61 {
62 MOVEIT_CLASS_FORWARD(InteractionHandler);
63 MOVEIT_CLASS_FORWARD(KinematicOptionsMap);
64 MOVEIT_CLASS_FORWARD(RobotInteraction);
65 
66 // Manage interactive markers for controlling a robot state.
67 //
68 // The RobotInteraction class manages one or more InteractionHandler objects
69 // each of which maintains a set of interactive markers for manipulating one
70 // group of one RobotState.
71 //
72 // The group being manipulated is common to all InteractionHandler objects
73 // contained in a RobotInteraction instance.
75 {
76 public:
77  // DEPRECATED TYPES. For backwards compatibility. Avoid using these.
78  typedef ::robot_interaction::InteractionHandler InteractionHandler;
79  typedef ::robot_interaction::InteractionHandlerPtr InteractionHandlerPtr;
80  typedef ::robot_interaction::EndEffectorInteraction EndEffector;
81  typedef ::robot_interaction::JointInteraction Joint;
82  typedef ::robot_interaction::GenericInteraction Generic;
83 
84 public:
86  static const std::string INTERACTIVE_MARKER_TOPIC;
87 
88  RobotInteraction(const robot_model::RobotModelConstPtr& kmodel, const std::string& ns = "");
89  virtual ~RobotInteraction();
90 
91  const std::string& getServerTopic(void) const
92  {
93  return topic_;
94  }
95 
107  void addActiveComponent(const InteractiveMarkerConstructorFn& construct, const ProcessFeedbackFn& process,
109  const std::string& name = "");
110 
119  void decideActiveComponents(const std::string& group);
120  void decideActiveComponents(const std::string& group, InteractionStyle::InteractionStyle style);
121 
124  void clear();
125 
131  void addInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
132  const double marker_scale = 0.0);
133 
134  // Update pose of all interactive markers to match the handler's RobotState.
135  // Call this when the handler's RobotState changes.
136  void updateInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler);
137 
138  // True if markers are being shown for this handler.
139  bool showingMarkers(const ::robot_interaction::InteractionHandlerPtr& handler);
140 
141  // Display all markers that have been added.
142  // This is needed after calls to addInteractiveMarkers() to publish the
143  // resulting markers so they get displayed. This call is not needed after
144  // calling updateInteractiveMarkers() which publishes the results itself.
145  void publishInteractiveMarkers();
146 
147  // Clear all interactive markers.
148  // This removes all interactive markers but does not affect which
149  // interactions are active. After this a call to publishInteractiveMarkers()
150  // is needed to actually remove the markers from the display.
151  void clearInteractiveMarkers();
152 
153  const robot_model::RobotModelConstPtr& getRobotModel() const
154  {
155  return robot_model_;
156  }
157 
158  // Get the kinematic options map.
159  // Use this to set kinematic options (defaults or per-group).
160  KinematicOptionsMapPtr getKinematicOptionsMap()
161  {
162  return kinematic_options_map_;
163  }
164 
165  // enable/disable subscription of the topics to move interactive marker
166  void toggleMoveInteractiveMarkerTopic(bool enable);
167 
168 private:
169  // called by decideActiveComponents(); add markers for end effectors
170  void decideActiveEndEffectors(const std::string& group);
171  void decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style);
172 
173  // called by decideActiveComponents(); add markers for planar and floating
174  // joints
175  void decideActiveJoints(const std::string& group);
176 
177  void moveInteractiveMarker(const std::string name, const geometry_msgs::PoseStampedConstPtr& msg);
178  // register the name of the topic and marker name to move
179  // interactive marker from other ROS nodes
180  void registerMoveInteractiveMarkerTopic(const std::string marker_name, const std::string& name);
181  // return the diameter of the sphere that certainly can enclose the AABB of the link
182  double computeLinkMarkerSize(const std::string& link);
183  // return the diameter of the sphere that certainly can enclose the AABB of
184  // the links in this group
185  double computeGroupMarkerSize(const std::string& group);
186  void computeMarkerPose(const ::robot_interaction::InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
187  const robot_state::RobotState& robot_state, geometry_msgs::Pose& pose,
188  geometry_msgs::Pose& control_to_eef_tf) const;
189 
190  void addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
191  const EndEffectorInteraction& eef, visualization_msgs::InteractiveMarker& im,
192  bool position = true, bool orientation = true);
193  void addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
194  const EndEffectorInteraction& eef, const geometry_msgs::Pose& offset,
195  visualization_msgs::InteractiveMarker& im, bool position = true, bool orientation = true);
196  void processInteractiveMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
197  void subscribeMoveInteractiveMarker(const std::string marker_name, const std::string& name);
198  void processingThread();
199  void clearInteractiveMarkersUnsafe();
200 
201  std::unique_ptr<boost::thread> processing_thread_;
203 
204  boost::condition_variable new_feedback_condition_;
205  std::map<std::string, visualization_msgs::InteractiveMarkerFeedbackConstPtr> feedback_map_;
206 
207  robot_model::RobotModelConstPtr robot_model_;
208 
209  std::vector<EndEffectorInteraction> active_eef_;
210  std::vector<JointInteraction> active_vj_;
211  std::vector<GenericInteraction> active_generic_;
212 
213  std::map<std::string, ::robot_interaction::InteractionHandlerPtr> handlers_;
214  std::map<std::string, std::size_t> shown_markers_;
215 
216  // This mutex is locked every time markers are read or updated;
217  // This includes the active_* arrays and shown_markers_
218  // Please note that this mutex *MUST NOT* be locked while operations
219  // on the interative marker server are called because the server
220  // also locks internally and we could othewrise end up with a problem
221  // of Thread 1: Lock A, Lock B, Unlock B, Unloack A
222  // Thread 2: Lock B, Lock A
223  // => deadlock
224  boost::mutex marker_access_lock_;
225 
227  // ros subscribers to move the interactive markers by other ros nodes
228  std::vector<ros::Subscriber> int_marker_move_subscribers_;
229  // the array of the names of the topics which need to be subscribed
230  // to move the interactive markers by other ROS nodes
231  std::vector<std::string> int_marker_move_topics_;
232  // the array of the marker names in the same order to int_marker_move_topics_
233  std::vector<std::string> int_marker_names_;
234 
235  std::string topic_;
236 
237  // options for doing IK
238  // Locking is done internally
239  KinematicOptionsMapPtr kinematic_options_map_;
240 
241 public:
242  // DEPRECATED. This is included for backwards compatibility.
243  // These classes/enums used to be subclasses of RobotInteraction. This
244  // allows client code to continue to work as if they are.
245 
249  {
250  EEF_POSITION_ARROWS = InteractionStyle::POSITION_ARROWS,
251  EEF_ORIENTATION_CIRCLES = InteractionStyle::ORIENTATION_CIRCLES,
252  EEF_POSITION_SPHERE = InteractionStyle::POSITION_SPHERE,
253  EEF_ORIENTATION_SPHERE = InteractionStyle::ORIENTATION_SPHERE,
254  EEF_POSITION_EEF = InteractionStyle::POSITION_EEF,
255  EEF_ORIENTATION_EEF = InteractionStyle::ORIENTATION_EEF,
258  EEF_ORIENTATION = InteractionStyle::ORIENTATION,
261  EEF_POSITION_NOSPHERE = InteractionStyle::POSITION_NOSPHERE,
262  EEF_ORIENTATION_NOSPHERE = InteractionStyle::ORIENTATION_NOSPHERE,
264  };
265  // DEPRECATED. Use InteractionStyle::InteractionStyle version.
266  void decideActiveComponents(const std::string& group, EndEffectorInteractionStyle style);
267  // DEPRECATED. Use InteractionStyle::InteractionStyle version.
268  void decideActiveEndEffectors(const std::string& group, EndEffectorInteractionStyle style);
269  // DEPRECATED
270  const std::vector<EndEffectorInteraction>& getActiveEndEffectors() const
271  {
272  return active_eef_;
273  }
274  // DEPRECATED
275  const std::vector<JointInteraction>& getActiveJoints() const
276  {
277  return active_vj_;
278  }
279  // DEPRECATED
280  static bool updateState(
281  robot_state::RobotState& state, const EndEffectorInteraction& eef, const geometry_msgs::Pose& pose,
282  unsigned int attempts, double ik_timeout,
283  const robot_state::GroupStateValidityCallbackFn& validity_callback = robot_state::GroupStateValidityCallbackFn(),
284  const kinematics::KinematicsQueryOptions& kinematics_query_options = kinematics::KinematicsQueryOptions());
285 };
286 }
287 
288 #endif
boost::condition_variable new_feedback_condition_
::robot_interaction::GenericInteraction Generic
std::vector< JointInteraction > active_vj_
::robot_interaction::InteractionHandlerPtr InteractionHandlerPtr
const std::vector< EndEffectorInteraction > & getActiveEndEffectors() const
std::map< std::string,::robot_interaction::InteractionHandlerPtr > handlers_
::robot_interaction::JointInteraction Joint
const std::string & getServerTopic(void) const
KinematicOptionsMapPtr getKinematicOptionsMap()
const std::vector< JointInteraction > & getActiveJoints() const
const robot_model::RobotModelConstPtr & getRobotModel() const
std::vector< ros::Subscriber > int_marker_move_subscribers_
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::vector< std::string > int_marker_names_
interactive_markers::InteractiveMarkerServer * int_marker_server_
robot_model::RobotModelConstPtr robot_model_
KinematicOptionsMapPtr kinematic_options_map_
std::vector< std::string > int_marker_move_topics_
std::map< std::string, visualization_msgs::InteractiveMarkerFeedbackConstPtr > feedback_map_
boost::function< bool(const robot_state::RobotState &, geometry_msgs::Pose &)> InteractiveMarkerUpdateFn
Definition: interaction.h:116
boost::function< bool(robot_state::RobotState &state, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)> ProcessFeedbackFn
Definition: interaction.h:105
boost::function< bool(const robot_state::RobotState &state, visualization_msgs::InteractiveMarker &marker)> InteractiveMarkerConstructorFn
Definition: interaction.h:90
::robot_interaction::InteractionHandler InteractionHandler
std::vector< EndEffectorInteraction > active_eef_
::robot_interaction::EndEffectorInteraction EndEffector
std::unique_ptr< boost::thread > processing_thread_
std::map< std::string, std::size_t > shown_markers_
std::vector< GenericInteraction > active_generic_
static const std::string INTERACTIVE_MARKER_TOPIC
The topic name on which the internal Interactive Marker Server operates.
MOVEIT_CLASS_FORWARD(InteractionHandler)


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