selection_manager.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 RVIZ_SELECTION_MANAGER_H
00031 #define RVIZ_SELECTION_MANAGER_H
00032 
00033 #include <map>
00034 
00035 #include <QObject>
00036 
00037 #include "forwards.h"
00038 #include "selection_handler.h"
00039 
00040 #include <boost/shared_ptr.hpp>
00041 #include <boost/unordered_map.hpp>
00042 #include <boost/thread/recursive_mutex.hpp>
00043 
00044 #include <OGRE/OgreTexture.h>
00045 #include <OGRE/OgreMaterial.h>
00046 #include <OGRE/OgreMaterialManager.h>
00047 #include <OGRE/OgreMovableObject.h>
00048 #include <OGRE/OgreRenderQueueListener.h>
00049 
00050 #include <vector>
00051 #include <set>
00052 
00053 namespace ros
00054 {
00055 class Publisher;
00056 }
00057 
00058 namespace Ogre
00059 {
00060 class SceneManager;
00061 class Viewport;
00062 class WireBoundingBox;
00063 class SceneNode;
00064 class Material;
00065 class PixelBox;
00066 class Rectangle2D;
00067 class MovableObject;
00068 }
00069 
00070 namespace rviz
00071 {
00072 class Object;
00073 class PropertyTreeModel;
00074 class ViewportMouseEvent;
00075 class VisualizationManager;
00076 
00077 class SelectionManager: public QObject, public Ogre::MaterialManager::Listener, public Ogre::RenderQueueListener
00078 {
00079 Q_OBJECT
00080 public:
00081   enum SelectType
00082   {
00083     Add,
00084     Remove,
00085     Replace
00086   };
00087 
00088   SelectionManager(VisualizationManager* manager);
00089   ~SelectionManager();
00090 
00091   void initialize();
00092 
00094   void setDebugMode( bool debug );
00095 
00096   void clearHandlers();
00097   void addObject( CollObjectHandle obj, SelectionHandler* handler );
00098   void removeObject(CollObjectHandle obj);
00099 
00100   // control the highlight box being displayed while selecting
00101   void highlight(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2);
00102   void removeHighlight();
00103 
00104   // select all objects in bounding box
00105   void select(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, SelectType type);
00106 
00107   // @return handles of all objects in the given bounding box
00108   // @param single_render_pass only perform one rendering pass (point cloud selecting won't work)
00109   void pick(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, M_Picked& results, bool single_render_pass=false );
00110 
00111   void update();
00112 
00113   // modify the list of currently selected objects
00114   void setSelection(const M_Picked& objs);
00115   void addSelection(const M_Picked& objs);
00116   void removeSelection(const M_Picked& objs);
00117   const M_Picked& getSelection() { return selection_; }
00118 
00119   SelectionHandler* getHandler( CollObjectHandle obj );
00120 
00121   static Ogre::ColourValue handleToColor( CollObjectHandle handle );
00122   //static CollObjectHandle colourToHandle( const Ogre::ColourValue & color );
00123   static void setPickColor( const Ogre::ColourValue& color, Ogre::SceneNode* node )       { setPickData( colorToHandle( color ), color, node ); }
00124   static void setPickColor( const Ogre::ColourValue& color, Ogre::MovableObject* object ) { setPickData( colorToHandle( color ), color, object ); }
00125   static void setPickHandle( CollObjectHandle handle, Ogre::SceneNode* node )             { setPickData( handle, handleToColor( handle ), node ); }
00126   static void setPickHandle( CollObjectHandle handle, Ogre::MovableObject* object )       { setPickData( handle, handleToColor( handle ), object ); }
00127   static void setPickData( CollObjectHandle handle, const Ogre::ColourValue& color, Ogre::SceneNode* node );
00128   static void setPickData( CollObjectHandle handle, const Ogre::ColourValue& color, Ogre::MovableObject* object );
00129 
00130   // if a material does not support the picking scheme, paint it black
00131   virtual Ogre::Technique* handleSchemeNotFound(unsigned short scheme_index,
00132       const Ogre::String& scheme_name,
00133       Ogre::Material* original_material,
00134       unsigned short lod_index,
00135       const Ogre::Renderable* rend);
00136 
00137   // create a new unique handle
00138   CollObjectHandle createHandle();
00139 
00140   // tell all handlers that interactive mode is active/inactive
00141   void enableInteraction( bool enable );
00142   bool getInteractionEnabled() { return interaction_enabled_; }
00143 
00144   // tell the view controller to look at the selection
00145   void focusOnSelection();
00146 
00147   // change the size of the off-screen selection buffer texture
00148   void setTextureSize( unsigned size );
00149 
00153   bool get3DPoint( Ogre::Viewport* viewport, const int x, const int y, Ogre::Vector3& result_point );
00154 
00171   bool get3DPatch( Ogre::Viewport* viewport, const int x, const int y, const unsigned width, 
00172                    const unsigned height, const bool skip_missing, 
00173                    std::vector<Ogre::Vector3> &result_points );
00174 
00175 
00191   bool getPatchDepthImage( Ogre::Viewport* viewport, const int x, const int y,  const unsigned width, 
00192                            const unsigned height, std::vector<float> & depth_vector);
00193 
00194   // Implementation for Ogre::RenderQueueListener.
00195   void renderQueueStarted( uint8_t queueGroupId,
00196                            const std::string& invocation, 
00197                            bool& skipThisInvocation );
00198 
00199   PropertyTreeModel* getPropertyModel() { return property_model_; }
00200 
00201 private Q_SLOTS:
00204   void updateProperties();
00205 
00206 private:
00207   void selectionAdded( const M_Picked& added );
00208   void selectionRemoved( const M_Picked& removed );
00209 
00210   std::pair<Picked, bool> addSelectedObject(const Picked& obj);
00211   void removeSelectedObject(const Picked& obj);
00212 
00213   void setHighlightRect(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2);
00214 
00216   void renderAndUnpack(Ogre::Viewport* viewport, uint32_t pass, int x1, int y1, int x2, int y2, V_CollObject& pixels);
00217 
00219   bool render( Ogre::Viewport* viewport, Ogre::TexturePtr tex,
00220                int x1, int y1, int x2, int y2,
00221                Ogre::PixelBox& dst_box, std::string material_scheme,
00222                unsigned texture_width, unsigned textured_height );
00223 
00224   void unpackColors(const Ogre::PixelBox& box, V_CollObject& pixels);
00225 
00226   void setDepthTextureSize(unsigned width, unsigned height);
00227 
00228   void publishDebugImage( const Ogre::PixelBox& pixel_box, const std::string& label );
00229 
00230   VisualizationManager* vis_manager_;
00231 
00232   boost::recursive_mutex global_mutex_;
00233 
00234   typedef boost::unordered_map<CollObjectHandle, SelectionHandler*> M_CollisionObjectToSelectionHandler;
00235   M_CollisionObjectToSelectionHandler objects_;
00236 
00237   bool highlight_enabled_;
00238 
00239   struct Highlight
00240   {
00241     int x1;
00242     int y1;
00243     int x2;
00244     int y2;
00245     Ogre::Viewport* viewport;
00246   };
00247   Highlight highlight_;
00248 
00249   M_Picked selection_;
00250 
00251   const static uint32_t s_num_render_textures_ = 2; // If you want to change this number to something > 3 you must provide more width for extra handles in the Picked structure (currently a u64)
00252   Ogre::TexturePtr render_textures_[s_num_render_textures_];
00253   Ogre::PixelBox pixel_boxes_[s_num_render_textures_];
00254 
00255   // Graphics card -based depth finding of clicked points.
00256   Ogre::TexturePtr depth_render_texture_;
00257   uint32_t depth_texture_width_, depth_texture_height_;
00258   Ogre::PixelBox depth_pixel_box_;
00259 
00260   uint32_t uid_counter_;
00261 
00262   Ogre::Rectangle2D* highlight_rectangle_;
00263   Ogre::SceneNode* highlight_node_;
00264   Ogre::Camera *camera_;
00265 
00266   V_CollObject pixel_buffer_;
00267 
00268   bool interaction_enabled_;
00269 
00270   bool debug_mode_;
00271 
00272   Ogre::MaterialPtr fallback_pick_material_;
00273   Ogre::Technique *fallback_pick_technique_;
00274   Ogre::Technique *fallback_black_technique_;
00275   Ogre::Technique *fallback_depth_technique_;
00276   Ogre::Technique *fallback_pick_cull_technique_;
00277   Ogre::Technique *fallback_black_cull_technique_;
00278   Ogre::Technique *fallback_depth_cull_technique_;
00279 
00280   uint32_t texture_size_;
00281 
00282   PropertyTreeModel* property_model_;
00283 
00284   typedef std::map<std::string, ros::Publisher> PublisherMap;
00285   PublisherMap debug_publishers_;
00286 };
00287 
00288 } // namespace rviz
00289 
00290 #endif // RVIZ_SELECTION_MANAGER_H


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