Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef RVIZ_INTERACTIVE_MARKER_H_
00031 #define RVIZ_INTERACTIVE_MARKER_H_
00032
00033 #include <boost/shared_ptr.hpp>
00034 #include <boost/thread/mutex.hpp>
00035 #include <boost/thread/recursive_mutex.hpp>
00036 #include <boost/thread/thread.hpp>
00037
00038 #include <OgreVector3.h>
00039 #include <OgreQuaternion.h>
00040
00041 #include <visualization_msgs/InteractiveMarker.h>
00042 #include <visualization_msgs/InteractiveMarkerPose.h>
00043 #include <visualization_msgs/InteractiveMarkerFeedback.h>
00044 #include <geometry_msgs/Pose.h>
00045
00046 #include <ros/publisher.h>
00047
00048 #include "rviz/selection/forwards.h"
00049 #include "rviz/ogre_helpers/axes.h"
00050
00051 #include "rviz/default_plugin/interactive_markers/interactive_marker_control.h"
00052 #include "rviz/properties/status_property.h"
00053
00054 namespace Ogre {
00055 class SceneNode;
00056 }
00057
00058 class QMenu;
00059
00060 namespace rviz
00061 {
00062 class DisplayContext;
00063 class InteractiveMarkerDisplay;
00064
00065 class InteractiveMarker : public QObject
00066 {
00067 Q_OBJECT
00068 public:
00069 InteractiveMarker( Ogre::SceneNode* scene_node, DisplayContext* context );
00070 virtual ~InteractiveMarker();
00071
00072
00073
00074 bool processMessage( const visualization_msgs::InteractiveMarker& message );
00075
00076
00077
00078 void processMessage( const visualization_msgs::InteractiveMarkerPose& message );
00079
00080
00081 void update(float wall_dt);
00082
00083
00084
00085 void setPose( Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name );
00086
00087 void translate( Ogre::Vector3 delta_position, const std::string &control_name );
00088 void rotate( Ogre::Quaternion delta_orientation, const std::string &control_name );
00089
00090
00091 void requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation );
00092
00093 void startDragging();
00094 void stopDragging();
00095
00096 const Ogre::Vector3& getPosition() { return position_; }
00097 const Ogre::Quaternion& getOrientation() { return orientation_; }
00098
00099 float getSize() { return scale_; }
00100 const std::string &getReferenceFrame() { return reference_frame_; }
00101 const std::string& getName() { return name_; }
00102
00103
00104 void setShowDescription( bool show );
00105
00106
00107 void setShowAxes( bool show );
00108
00109
00110 void setShowVisualAids( bool show );
00111
00112
00113 bool handleMouseEvent(ViewportMouseEvent& event, const std::string &control_name );
00114
00124 bool handle3DCursorEvent(ViewportMouseEvent& event, const Ogre::Vector3& cursor_pos, const Ogre::Quaternion& cursor_rot, const std::string &control_name);
00125
00126
00135 void showMenu( ViewportMouseEvent& event, const std::string &control_name, const Ogre::Vector3 &three_d_point, bool valid_point );
00136
00137
00138 void publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback,
00139 bool mouse_point_valid = false,
00140 const Ogre::Vector3& mouse_point_rel_world = Ogre::Vector3(0,0,0) );
00141
00142 bool hasMenu() { return has_menu_; }
00143
00145 boost::shared_ptr<QMenu> getMenu() { return menu_; }
00146
00147 Q_SIGNALS:
00148
00149 void userFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback);
00150 void statusUpdate( StatusProperty::Level level, const std::string& name, const std::string& text );
00151
00152 protected Q_SLOTS:
00153 void handleMenuSelect( int menu_item_id );
00154
00155 protected:
00156
00157 void publishPose();
00158
00159 void reset();
00160
00161
00162 void updateReferencePose();
00163
00164 QString makeMenuString( const std::string &entry );
00165
00166
00167
00168
00169 void populateMenu( QMenu* menu, std::vector<uint32_t>& ids );
00170
00171 DisplayContext* context_;
00172
00173
00174 std::string reference_frame_;
00175 ros::Time reference_time_;
00176 bool frame_locked_;
00177
00178
00179 Ogre::SceneNode *reference_node_;
00180
00181
00182 Ogre::Vector3 position_;
00183 Ogre::Quaternion orientation_;
00184
00185
00186 bool pose_changed_;
00187 double time_since_last_feedback_;
00188
00189 typedef boost::shared_ptr<InteractiveMarkerControl> InteractiveMarkerControlPtr;
00190 typedef std::map<std::string, InteractiveMarkerControlPtr> M_ControlPtr;
00191 M_ControlPtr controls_;
00192
00193 std::string name_;
00194 std::string description_;
00195
00196 bool dragging_;
00197
00198
00199 bool pose_update_requested_;
00200 Ogre::Vector3 requested_position_;
00201 Ogre::Quaternion requested_orientation_;
00202
00203 float scale_;
00204
00205 boost::shared_ptr<QMenu> menu_;
00206 bool has_menu_;
00207
00208
00209 struct MenuNode
00210 {
00211 visualization_msgs::MenuEntry entry;
00212 std::vector<uint32_t> child_ids;
00213 };
00214
00215
00216 std::map< uint32_t, MenuNode > menu_entries_;
00217
00218
00219 std::vector<uint32_t> top_level_menu_ids_;
00220
00221
00222 std::string last_control_name_;
00223
00224 double heart_beat_t_;
00225
00226
00227
00228 Axes *axes_;
00229
00230 InteractiveMarkerControlPtr description_control_;
00231
00232 std::string topic_ns_;
00233 std::string client_id_;
00234
00235 boost::recursive_mutex mutex_;
00236
00237 boost::shared_ptr< boost::thread > sys_thread_;
00238
00239 bool got_3d_point_for_menu_;
00240 Ogre::Vector3 three_d_point_for_menu_;
00241
00242 bool show_visual_aids_;
00243 };
00244
00245
00246 }
00247
00248 #endif