$search
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 #include <visualization_msgs/InteractiveMarkerControl.h> 00034 00035 #include "rviz/default_plugin/markers/marker_base.h" 00036 #include "rviz/selection/forwards.h" 00037 #include "rviz/viewport_mouse_event.h" 00038 00039 #include <OGRE/OgreRay.h> 00040 #include <OGRE/OgreVector3.h> 00041 #include <OGRE/OgreQuaternion.h> 00042 #include <OGRE/OgreSceneManager.h> 00043 00044 #include <boost/shared_ptr.hpp> 00045 00046 namespace Ogre 00047 { 00048 class SceneNode; 00049 } 00050 00051 namespace rviz 00052 { 00053 class VisualizationManager; 00054 class InteractiveMarker; 00055 class PointsMarker; 00056 00060 class InteractiveMarkerControl : public Ogre::SceneManager::Listener 00061 { 00062 public: 00063 00064 InteractiveMarkerControl(VisualizationManager* vis_manager, 00065 const visualization_msgs::InteractiveMarkerControl &message, 00066 Ogre::SceneNode *reference_node, InteractiveMarker *parent ); 00067 00068 virtual ~InteractiveMarkerControl(); 00069 00070 // called when interactive mode is globally switched on/off 00071 virtual void enableInteraction(bool enable); 00072 00073 // will receive all mouse events while the handler has focus 00074 virtual void handleMouseEvent(ViewportMouseEvent& event); 00075 00081 void interactiveMarkerPoseChanged( Ogre::Vector3 int_marker_position, Ogre::Quaternion int_marker_orientation ); 00082 00083 bool isInteractive() { return interaction_mode_ != visualization_msgs::InteractiveMarkerControl::NONE; } 00084 00085 // Called every frame by parent's update() function. 00086 void update(); 00087 00088 void setVisible( bool visible ); 00089 00090 protected: 00091 00092 // when this is called, we will face the camera 00093 virtual void preFindVisibleObjects(Ogre::SceneManager *source, Ogre::SceneManager::IlluminationRenderStage irs, Ogre::Viewport *v); 00094 00097 void rotate(Ogre::Ray &mouse_ray); 00098 00101 void moveRotate( Ogre::Ray &mouse_ray ); 00102 00104 void movePlane(Ogre::Ray &mouse_ray); 00105 00106 // Move the position along the control ray given the latest mouse ray. 00107 void moveAxis( const Ogre::Ray& mouse_ray, const ViewportMouseEvent& event ); 00108 00110 bool intersectYzPlane( const Ogre::Ray& mouse_ray, 00111 Ogre::Vector3& intersection_3d, 00112 Ogre::Vector2& intersection_2d, 00113 float& ray_t ); 00114 00116 bool intersectSomeYzPlane( const Ogre::Ray& mouse_ray, 00117 const Ogre::Vector3& point_in_plane, 00118 const Ogre::Quaternion& plane_orientation, 00119 Ogre::Vector3& intersection_3d, 00120 Ogre::Vector2& intersection_2d, 00121 float& ray_t ); 00122 00126 bool findClosestPoint( const Ogre::Ray& target_ray, 00127 const Ogre::Ray& mouse_ray, 00128 Ogre::Vector3& closest_point ); 00129 00132 void worldToScreen( const Ogre::Vector3& pos_rel_reference, 00133 const Ogre::Viewport* viewport, 00134 Ogre::Vector2& screen_pos ); 00135 00137 void addHighlightPass( S_MaterialPtr materials ); 00138 00139 // set the highlight color to (a,a,a) 00140 void setHighlight( float a ); 00141 00142 // Save a copy of the latest mouse event with the event type set to 00143 // wxEVT_MOTION, so that update() can resend the mouse event during 00144 // drag actions to maintain consistent behavior. 00145 void recordDraggingInPlaceEvent( ViewportMouseEvent& event ); 00146 00147 // Motion part of mouse event handling. 00148 void handleMouseMovement( ViewportMouseEvent& event ); 00149 00150 // Return closest point on a line to a test point. 00151 Ogre::Vector3 closestPointOnLineToPoint( const Ogre::Vector3& line_start, 00152 const Ogre::Vector3& line_dir, 00153 const Ogre::Vector3& test_point ); 00154 00155 bool dragging_; 00156 00157 ViewportMouseEvent dragging_in_place_event_; 00158 00159 VisualizationManager* vis_manager_; 00160 00161 CollObjectHandle coll_object_handle_; 00162 00165 Ogre::SceneNode *reference_node_; 00166 00173 Ogre::SceneNode *control_frame_node_; 00174 00175 // this is a child of scene_node, but might be oriented differently 00176 Ogre::SceneNode *markers_node_; 00177 00178 // interaction mode 00179 int interaction_mode_; 00180 int orientation_mode_; 00181 00182 // if in view facing mode, the markers should be 00183 // view facing as well 00184 // if set to false, they will follow the parent's transformations 00185 bool independent_marker_orientation_; 00186 00191 Ogre::Quaternion control_orientation_; 00192 00193 bool always_visible_; 00194 00195 std::string description_; 00196 00197 std::string name_; 00198 00199 typedef boost::shared_ptr<MarkerBase> MarkerBasePtr; 00200 std::vector< MarkerBasePtr > markers_; 00201 00202 InteractiveMarker *parent_; 00203 00204 std::set<Ogre::Pass*> highlight_passes_; 00205 00206 // PointsMarkers are rendered by special shader programs, so the 00207 // regular highlighting method does not work for them. Keep a 00208 // vector of them so we can call their setHighlightColor() function. 00209 typedef boost::shared_ptr<PointsMarker> PointsMarkerPtr; 00210 std::vector< PointsMarkerPtr > points_markers_; 00211 00214 Ogre::Radian rotation_; 00215 00219 Ogre::Radian rotation_at_mouse_down_; 00220 00221 Ogre::Quaternion intitial_orientation_; 00222 00225 Ogre::Vector3 grab_point_; 00226 00227 // The 2D position in pixel coordinates of the mouse-down location. 00228 Ogre::Vector2 grab_pixel_; 00229 // The position of the parent when the mouse button is pressed. 00230 Ogre::Vector3 parent_position_at_mouse_down_; 00231 00234 Ogre::Quaternion control_frame_orientation_at_mouse_down_; 00235 00238 Ogre::Quaternion parent_orientation_at_mouse_down_; 00239 00243 Ogre::Vector3 rotation_axis_; 00244 00247 Ogre::Vector3 rotation_center_rel_control_; 00248 00251 Ogre::Vector3 grab_point_rel_control_; 00252 00253 bool has_focus_; 00254 bool interaction_enabled_; 00255 00256 bool visible_; 00257 }; 00258 00259 } 00260 00261 #endif /* INTERACTIVE_MARKER_CONTROL_H_ */