interactive_marker.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef RVIZ_INTERACTIVE_MARKER_H_
31 #define RVIZ_INTERACTIVE_MARKER_H_
32 
33 #ifndef Q_MOC_RUN
34 #include <boost/shared_ptr.hpp>
35 #include <boost/thread/mutex.hpp>
36 #include <boost/thread/recursive_mutex.hpp>
37 #include <boost/thread/thread.hpp>
38 
39 #include <OgreVector3.h>
40 #include <OgreQuaternion.h>
41 #endif
42 
43 #include <visualization_msgs/InteractiveMarker.h>
44 #include <visualization_msgs/InteractiveMarkerPose.h>
45 #include <visualization_msgs/InteractiveMarkerFeedback.h>
46 #include <geometry_msgs/Pose.h>
47 
48 #include <ros/publisher.h>
49 
51 #include "rviz/ogre_helpers/axes.h"
52 
55 
56 namespace Ogre {
57 class SceneNode;
58 }
59 
60 class QMenu;
61 
62 namespace rviz
63 {
64 class DisplayContext;
65 class InteractiveMarkerDisplay;
66 
67 class InteractiveMarker : public QObject
68 {
69 Q_OBJECT
70 public:
71  InteractiveMarker( Ogre::SceneNode* scene_node, DisplayContext* context );
72  virtual ~InteractiveMarker();
73 
74  // reset contents to reflect the data from a new message
75  // @return success
76  bool processMessage( const visualization_msgs::InteractiveMarker& message );
77 
78  // reset contents to reflect the data from a new message
79  // @return success
80  void processMessage( const visualization_msgs::InteractiveMarkerPose& message );
81 
82  // called every frame update
83  void update(float wall_dt);
84 
85  // directly set the pose, relative to parent frame
86  // if publish is set to true, publish the change
87  void setPose( Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name );
88 
89  void translate( Ogre::Vector3 delta_position, const std::string &control_name );
90  void rotate( Ogre::Quaternion delta_orientation, const std::string &control_name );
91 
92  // schedule a pose reset once dragging is finished
93  void requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation );
94 
95  void startDragging();
96  void stopDragging();
97 
98  const Ogre::Vector3& getPosition() { return position_; }
99  const Ogre::Quaternion& getOrientation() { return orientation_; }
100 
101  float getSize() { return scale_; }
102  const std::string &getReferenceFrame() { return reference_frame_; }
103  const std::string& getName() { return name_; }
104 
105  // show name above marker
106  void setShowDescription( bool show );
107 
108  // show axes in origin
109  void setShowAxes( bool show );
110 
111  // show visual helpers while dragging
112  void setShowVisualAids( bool show );
113 
114  // @return true if the mouse event was intercepted, false if it was ignored
115  bool handleMouseEvent(ViewportMouseEvent& event, const std::string &control_name );
116 
126  bool handle3DCursorEvent(ViewportMouseEvent& event, const Ogre::Vector3& cursor_pos, const Ogre::Quaternion& cursor_rot, const std::string &control_name);
127 
128 
137  void showMenu( ViewportMouseEvent& event, const std::string &control_name, const Ogre::Vector3 &three_d_point, bool valid_point );
138 
139  // fill in current marker pose & name, publish
140  void publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback,
141  bool mouse_point_valid = false,
142  const Ogre::Vector3& mouse_point_rel_world = Ogre::Vector3(0,0,0) );
143 
144  bool hasMenu() { return has_menu_; }
145 
147  boost::shared_ptr<QMenu> getMenu() { return menu_; }
148 
149 Q_SIGNALS:
150 
151 void userFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback);
152 void statusUpdate( StatusProperty::Level level, const std::string& name, const std::string& text );
153 
154 protected Q_SLOTS:
155  void handleMenuSelect( int menu_item_id );
156 
157 protected:
158 
159  void publishPose();
160 
161  void reset();
162 
163  // set the pose of the parent frame, relative to the fixed frame
164  void updateReferencePose();
165 
166  QString makeMenuString( const std::string &entry );
167 
168  // Recursively append menu and submenu entries to menu, based on a
169  // vector of menu entry id numbers describing the menu entries at the
170  // current level.
171  void populateMenu( QMenu* menu, std::vector<uint32_t>& ids );
172 
174 
175  // pose of parent coordinate frame
176  std::string reference_frame_;
179 
180  // node representing reference frame in tf, like /map, /base_link, /head, etc.
181  Ogre::SceneNode *reference_node_;
182 
183  // pose being controlled, relative to reference frame
184  Ogre::Vector3 position_;
185  Ogre::Quaternion orientation_;
186 
187  // has the pose changed since the last feedback was sent?
190 
192  typedef std::map<std::string, InteractiveMarkerControlPtr> M_ControlPtr;
193  M_ControlPtr controls_;
194 
195  std::string name_;
196  std::string description_;
197 
198  bool dragging_;
199 
200  // pose being controlled
202  Ogre::Vector3 requested_position_;
203  Ogre::Quaternion requested_orientation_;
204 
205  float scale_;
206 
208  bool has_menu_;
209 
210  // Helper to more simply represent the menu tree.
211  struct MenuNode
212  {
213  visualization_msgs::MenuEntry entry;
214  std::vector<uint32_t> child_ids;
215  };
216 
217  // maps menu index to menu entry and item
218  std::map< uint32_t, MenuNode > menu_entries_;
219 
220  // Helper to store the top level of the menu tree.
221  std::vector<uint32_t> top_level_menu_ids_;
222 
223  // which control has popped up the menu
224  std::string last_control_name_;
225 
227 
228  // visual aids
229 
231 
232  InteractiveMarkerControlPtr description_control_;
233 
234  std::string topic_ns_;
235  std::string client_id_;
236 
237  boost::recursive_mutex mutex_;
238 
240 
242  Ogre::Vector3 three_d_point_for_menu_;
243 
245 };
246 
247 
248 }
249 
250 #endif /* INTERACTIVE_MARKER_H_ */
boost::recursive_mutex mutex_
std::map< uint32_t, MenuNode > menu_entries_
const Ogre::Quaternion & getOrientation()
Ogre::Vector3 three_d_point_for_menu_
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
const Ogre::Vector3 & getPosition()
Pure-virtual base class for objects which give Display subclasses context in which to work...
const std::string & getName()
std::map< std::string, InteractiveMarkerControlPtr > M_ControlPtr
Ogre::Quaternion requested_orientation_
InteractiveMarkerControlPtr description_control_
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:58
visualization_msgs::MenuEntry entry
std::vector< uint32_t > top_level_menu_ids_
boost::shared_ptr< boost::thread > sys_thread_
boost::shared_ptr< InteractiveMarkerControl > InteractiveMarkerControlPtr
boost::shared_ptr< QMenu > getMenu()
const std::string & getReferenceFrame()
TFSIMD_FORCE_INLINE Vector3 rotate(const Vector3 &wAxis, const tfScalar angle) const
boost::shared_ptr< QMenu > menu_
Ogre::Quaternion orientation_
Ogre::SceneNode * reference_node_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51