interactive_marker_control.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef INTERACTIVE_MARKER_CONTROL_H_
00031 #define INTERACTIVE_MARKER_CONTROL_H_
00032 
00033 
00034 #include <boost/shared_ptr.hpp>
00035 #include <boost/enable_shared_from_this.hpp>
00036 
00037 #include <QCursor>
00038 
00039 #include <OGRE/OgreRay.h>
00040 #include <OGRE/OgreVector3.h>
00041 #include <OGRE/OgreQuaternion.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 
00044 #include <visualization_msgs/InteractiveMarkerControl.h>
00045 
00046 #include "rviz/default_plugin/markers/marker_base.h"
00047 #include "rviz/selection/forwards.h"
00048 #include "rviz/viewport_mouse_event.h"
00049 #include "rviz/interactive_object.h"
00050 
00051 namespace Ogre
00052 {
00053   class SceneNode;
00054 }
00055 
00056 namespace rviz
00057 {
00058 class DisplayContext;
00059 class InteractiveMarker;
00060 class PointsMarker;
00061 class Line;
00062 
00066 class InteractiveMarkerControl: public Ogre::SceneManager::Listener,
00067                                 public InteractiveObject,
00068                                 public boost::enable_shared_from_this<InteractiveMarkerControl>
00069 {
00070 public:
00078   InteractiveMarkerControl( DisplayContext* context,
00079                             Ogre::SceneNode *reference_node,
00080                             InteractiveMarker *parent );
00081 
00082   virtual ~InteractiveMarkerControl();
00083 
00086   void processMessage( const visualization_msgs::InteractiveMarkerControl &message );
00087 
00088   // called when interactive mode is globally switched on/off
00089   virtual void enableInteraction(bool enable);
00090 
00091   // will receive all mouse events while the handler has focus
00092   virtual void handleMouseEvent(ViewportMouseEvent& event);
00093 
00119   virtual void handle3DCursorEvent( ViewportMouseEvent event, const Ogre::Vector3& cursor_3D_pos, const Ogre::Quaternion& cursor_3D_orientation);
00120 
00126   void interactiveMarkerPoseChanged( Ogre::Vector3 int_marker_position, Ogre::Quaternion int_marker_orientation );
00127 
00128   bool isInteractive() { return interaction_mode_ != visualization_msgs::InteractiveMarkerControl::NONE; }
00129 
00130   // Called every frame by parent's update() function.
00131   void update();
00132 
00133   void setVisible( bool visible );
00134 
00135   bool getVisible();
00136 
00137   // Highlight types
00138   enum ControlHighlight { NO_HIGHLIGHT = 0,
00139          HOVER_HIGHLIGHT = 3,
00140          ACTIVE_HIGHLIGHT = 5};
00141 
00142   // Public access to highlight controls
00143   void setHighlight( const ControlHighlight &hl  );
00144 
00148   InteractiveMarker* getParent() { return parent_ ;}
00149 
00153   const std::string& getName() { return name_; }
00154 
00158   const QString& getDescription() { return description_; }
00159 
00163   int getInteractionMode() { return interaction_mode_; }
00164 
00168   int getOrientationMode() { return orientation_mode_; }
00169 
00173   void setShowVisualAids( bool show ) { show_visual_aids_ = show; }
00174 
00175 protected:
00176 
00177   // when this is called, we will face the camera
00178   virtual void preFindVisibleObjects(Ogre::SceneManager *source, Ogre::SceneManager::IlluminationRenderStage irs, Ogre::Viewport *v);
00179 
00180   void updateControlOrientationForViewFacing( Ogre::Viewport* v );
00181 
00184   Ogre::Ray getMouseRayInReferenceFrame( const ViewportMouseEvent& event, int x, int y );
00185 
00187   void beginRelativeMouseMotion( const ViewportMouseEvent& event );
00188 
00191   bool getRelativeMouseMotion( const ViewportMouseEvent& event, int& dx, int& dy );
00192 
00194   void rotateXYRelative( const ViewportMouseEvent& event );
00195 
00197   void rotateZRelative( const ViewportMouseEvent& event );
00198 
00200   void moveZAxisRelative( const ViewportMouseEvent& event );
00201 
00203   void moveZAxisWheel( const ViewportMouseEvent& event );
00204 
00206   void moveViewPlane( Ogre::Ray &mouse_ray, const ViewportMouseEvent& event );
00207 
00210   void rotate( Ogre::Ray &mouse_ray );
00211 
00213   void rotate(const Ogre::Vector3& cursor_position_in_reference_frame);
00214 
00217   void moveRotate( Ogre::Ray &mouse_ray );
00218 
00220   void moveRotate( const Ogre::Vector3& cursor_position_in_reference_frame, bool lock_axis = true);
00221 
00224   void movePlane( Ogre::Ray &mouse_ray );
00225 
00227   void movePlane( const Ogre::Vector3& cursor_position_in_reference_frame );
00228 
00231   void moveAxis( const Ogre::Ray& mouse_ray, const ViewportMouseEvent& event );
00232 
00234   void moveAxis( const Ogre::Vector3& cursor_position_in_reference_frame );
00235 
00237   void move3D( const Ogre::Vector3& cursor_position_in_reference_frame,
00238                const Ogre::Quaternion &cursor_orientation_in_reference_frame );
00239 
00241   void rotate3D( const Ogre::Vector3& cursor_position_in_reference_frame,
00242                  const Ogre::Quaternion &cursor_orientation_in_reference_frame );
00243 
00245   void moveRotate3D( const Ogre::Vector3& cursor_position_in_reference_frame,
00246                      const Ogre::Quaternion& cursor_orientation_in_reference_frame );
00247 
00249   bool intersectYzPlane( const Ogre::Ray& mouse_ray,
00250                          Ogre::Vector3& intersection_3d,
00251                          Ogre::Vector2& intersection_2d,
00252                          float& ray_t );
00253 
00255   bool intersectSomeYzPlane( const Ogre::Ray& mouse_ray,
00256                              const Ogre::Vector3& point_in_plane,
00257                              const Ogre::Quaternion& plane_orientation,
00258                              Ogre::Vector3& intersection_3d,
00259                              Ogre::Vector2& intersection_2d,
00260                              float& ray_t );
00261 
00265   bool findClosestPoint( const Ogre::Ray& target_ray,
00266                          const Ogre::Ray& mouse_ray,
00267                          Ogre::Vector3& closest_point );
00268 
00271   void worldToScreen( const Ogre::Vector3& pos_rel_reference,
00272                       const Ogre::Viewport* viewport,
00273                       Ogre::Vector2& screen_pos );
00274 
00276   void addHighlightPass( S_MaterialPtr materials );
00277 
00278   // set the highlight color to (a,a,a)
00279   void setHighlight( float a );
00280 
00281   // Save a copy of the latest mouse event with the event type set to
00282   // QEvent::MouseMove, so that update() can resend the mouse event during
00283   // drag actions to maintain consistent behavior.
00284   void recordDraggingInPlaceEvent( ViewportMouseEvent& event );
00285 
00286   // Begin a new mouse motion.  Called when left button is pressed to begin a drag.
00287   void beginMouseMovement( ViewportMouseEvent& event, bool line_visible );
00288 
00289   // Motion part of mouse event handling.
00290   void handleMouseMovement( ViewportMouseEvent& event );
00291 
00292   // Mouse wheel part of mouse event handling.
00293   void handleMouseWheelMovement( ViewportMouseEvent& event );
00294 
00295   // Return closest point on a line to a test point.
00296   Ogre::Vector3 closestPointOnLineToPoint( const Ogre::Vector3& line_start,
00297                                            const Ogre::Vector3& line_dir,
00298                                            const Ogre::Vector3& test_point );
00299 
00301   void makeMarkers( const visualization_msgs::InteractiveMarkerControl &message );
00302 
00303   void stopDragging( bool force = false );
00304 
00305   virtual const QCursor& getCursor() const { return cursor_; }
00306 
00307   bool mouse_dragging_;
00308   Ogre::Viewport* drag_viewport_;
00309 
00310   ViewportMouseEvent dragging_in_place_event_;
00311 
00312   DisplayContext* context_;
00313 
00314   CollObjectHandle coll_object_handle_;
00315 
00318   Ogre::SceneNode *reference_node_;
00319 
00326   Ogre::SceneNode *control_frame_node_;
00327 
00328   // this is a child of scene_node, but might be oriented differently
00329   Ogre::SceneNode *markers_node_;
00330 
00331   // interaction mode
00332   int interaction_mode_;
00333   int orientation_mode_;
00334 
00335   // if in view facing mode, the markers should be
00336   // view facing as well
00337   // if set to false, they will follow the parent's transformations
00338   bool independent_marker_orientation_;
00339 
00344   Ogre::Quaternion control_orientation_;
00345 
00346   bool always_visible_;
00347 
00348   QString description_;
00349 
00350   std::string name_;
00351 
00352   typedef boost::shared_ptr<MarkerBase> MarkerBasePtr;
00353   std::vector< MarkerBasePtr > markers_;
00354 
00355   InteractiveMarker *parent_;
00356 
00357   std::set<Ogre::Pass*> highlight_passes_;
00358 
00359   // PointsMarkers are rendered by special shader programs, so the
00360   // regular highlighting method does not work for them.  Keep a
00361   // vector of them so we can call their setHighlightColor() function.
00362   typedef boost::shared_ptr<PointsMarker> PointsMarkerPtr;
00363   std::vector< PointsMarkerPtr > points_markers_;
00364 
00367   Ogre::Radian rotation_;
00368 
00372   Ogre::Radian rotation_at_mouse_down_;
00373 
00376   Ogre::Vector3 grab_point_in_reference_frame_;
00377 
00380   Ogre::Quaternion grab_orientation_in_reference_frame_;
00381 
00384   Ogre::Vector3 parent_to_cursor_in_cursor_frame_at_grab_;
00385 
00388   Ogre::Quaternion rotation_cursor_to_parent_at_grab_;
00389 
00391   Qt::KeyboardModifiers modifiers_at_drag_begin_;
00392 
00394   int mouse_x_at_drag_begin_;
00395   int mouse_y_at_drag_begin_;
00396 
00397   /* mouse ray when drag begins. */
00398   Ogre::Ray mouse_ray_at_drag_begin_;
00399 
00400   /* how far to move in Z when mouse moves 1 pixel. */
00401   double mouse_z_scale_;
00402 
00404   int mouse_relative_to_absolute_x_;
00405   int mouse_relative_to_absolute_y_;
00406 
00408   Ogre::Vector3 parent_to_grab_position_;
00409 
00411   Ogre::Vector3 parent_position_at_mouse_down_;
00412 
00415   Ogre::Quaternion control_frame_orientation_at_mouse_down_;
00416 
00419   Ogre::Quaternion parent_orientation_at_mouse_down_;
00420 
00424   Ogre::Vector3 rotation_axis_;
00425 
00428   Ogre::Vector3 rotation_center_rel_control_;
00429 
00432   Ogre::Vector3 grab_point_rel_control_;
00433 
00434   bool has_focus_;
00435   bool interaction_enabled_;
00436 
00437   bool visible_;
00438   bool view_facing_;
00439 
00440   QCursor cursor_;
00441   QString status_msg_;
00442 
00443   bool mouse_down_;
00444 
00445   boost::shared_ptr<Line> line_;
00446 
00447   bool show_visual_aids_;
00448 };
00449 
00450 }
00451 
00452 #endif /* INTERACTIVE_MARKER_CONTROL_H_ */


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:35