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_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 <OgreTexture.h>
00045 #include <OgreMaterial.h>
00046 #include <OgreMaterialManager.h>
00047 #include <OgreMovableObject.h>
00048 #include <OgreRenderQueueListener.h>
00049 #include <OgreSharedPtr.h>
00050
00051 #include <vector>
00052 #include <set>
00053
00054 namespace ros
00055 {
00056 class Publisher;
00057 }
00058
00059 namespace Ogre
00060 {
00061 class SceneManager;
00062 class Viewport;
00063 class WireBoundingBox;
00064 class SceneNode;
00065 class Material;
00066 class PixelBox;
00067 class Rectangle2D;
00068 class MovableObject;
00069 }
00070
00071 namespace rviz
00072 {
00073 class Object;
00074 class PropertyTreeModel;
00075 class ViewportMouseEvent;
00076 class VisualizationManager;
00077
00078 class SelectionManager: public QObject, public Ogre::MaterialManager::Listener, public Ogre::RenderQueueListener
00079 {
00080 Q_OBJECT
00081 public:
00082 enum SelectType
00083 {
00084 Add,
00085 Remove,
00086 Replace
00087 };
00088
00089 SelectionManager(VisualizationManager* manager);
00090 ~SelectionManager();
00091
00092 void initialize();
00093
00095 void setDebugMode( bool debug );
00096
00097 void clearHandlers();
00098 void addObject( CollObjectHandle obj, SelectionHandler* handler );
00099 void removeObject(CollObjectHandle obj);
00100
00101
00102 void highlight(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2);
00103 void removeHighlight();
00104
00105
00106 void select(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, SelectType type);
00107
00108
00109
00110 void pick(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, M_Picked& results, bool single_render_pass=false );
00111
00112 void update();
00113
00114
00115 void setSelection(const M_Picked& objs);
00116 void addSelection(const M_Picked& objs);
00117 void removeSelection(const M_Picked& objs);
00118 const M_Picked& getSelection() { return selection_; }
00119
00120 SelectionHandler* getHandler( CollObjectHandle obj );
00121
00122 static Ogre::ColourValue handleToColor( CollObjectHandle handle );
00123
00124 static void setPickColor( const Ogre::ColourValue& color, Ogre::SceneNode* node ) { setPickData( colorToHandle( color ), color, node ); }
00125 static void setPickColor( const Ogre::ColourValue& color, Ogre::MovableObject* object ) { setPickData( colorToHandle( color ), color, object ); }
00126 static void setPickHandle( CollObjectHandle handle, Ogre::SceneNode* node ) { setPickData( handle, handleToColor( handle ), node ); }
00127 static void setPickHandle( CollObjectHandle handle, Ogre::MovableObject* object ) { setPickData( handle, handleToColor( handle ), object ); }
00128 static void setPickData( CollObjectHandle handle, const Ogre::ColourValue& color, Ogre::SceneNode* node );
00129 static void setPickData( CollObjectHandle handle, const Ogre::ColourValue& color, Ogre::MovableObject* object );
00130
00131
00132 virtual Ogre::Technique* handleSchemeNotFound(unsigned short scheme_index,
00133 const Ogre::String& scheme_name,
00134 Ogre::Material* original_material,
00135 unsigned short lod_index,
00136 const Ogre::Renderable* rend);
00137
00138
00139 CollObjectHandle createHandle();
00140
00141
00142 void enableInteraction( bool enable );
00143 bool getInteractionEnabled() { return interaction_enabled_; }
00144
00145
00146 void focusOnSelection();
00147
00148
00149 void setTextureSize( unsigned size );
00150
00154 bool get3DPoint( Ogre::Viewport* viewport, const int x, const int y, Ogre::Vector3& result_point );
00155
00172 bool get3DPatch( Ogre::Viewport* viewport, const int x, const int y, const unsigned width,
00173 const unsigned height, const bool skip_missing,
00174 std::vector<Ogre::Vector3> &result_points );
00175
00176
00192 bool getPatchDepthImage( Ogre::Viewport* viewport, const int x, const int y, const unsigned width,
00193 const unsigned height, std::vector<float> & depth_vector);
00194
00195
00196 void renderQueueStarted( uint8_t queueGroupId,
00197 const std::string& invocation,
00198 bool& skipThisInvocation );
00199
00200 PropertyTreeModel* getPropertyModel() { return property_model_; }
00201
00202 private Q_SLOTS:
00205 void updateProperties();
00206
00207 private:
00208 void selectionAdded( const M_Picked& added );
00209 void selectionRemoved( const M_Picked& removed );
00210
00211 std::pair<Picked, bool> addSelectedObject(const Picked& obj);
00212 void removeSelectedObject(const Picked& obj);
00213
00214 void setHighlightRect(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2);
00215
00217 void renderAndUnpack(Ogre::Viewport* viewport, uint32_t pass, int x1, int y1, int x2, int y2, V_CollObject& pixels);
00218
00220 bool render( Ogre::Viewport* viewport, Ogre::TexturePtr tex,
00221 int x1, int y1, int x2, int y2,
00222 Ogre::PixelBox& dst_box, std::string material_scheme,
00223 unsigned texture_width, unsigned textured_height );
00224
00225 void unpackColors(const Ogre::PixelBox& box, V_CollObject& pixels);
00226
00227 void setDepthTextureSize(unsigned width, unsigned height);
00228
00229 void publishDebugImage( const Ogre::PixelBox& pixel_box, const std::string& label );
00230
00231 VisualizationManager* vis_manager_;
00232
00233 boost::recursive_mutex global_mutex_;
00234
00235 typedef boost::unordered_map<CollObjectHandle, SelectionHandler*> M_CollisionObjectToSelectionHandler;
00236 M_CollisionObjectToSelectionHandler objects_;
00237
00238 bool highlight_enabled_;
00239
00240 struct Highlight
00241 {
00242 int x1;
00243 int y1;
00244 int x2;
00245 int y2;
00246 Ogre::Viewport* viewport;
00247 };
00248 Highlight highlight_;
00249
00250 M_Picked selection_;
00251
00252 const static uint32_t s_num_render_textures_ = 2;
00253 Ogre::TexturePtr render_textures_[s_num_render_textures_];
00254 Ogre::PixelBox pixel_boxes_[s_num_render_textures_];
00255
00256
00257 Ogre::TexturePtr depth_render_texture_;
00258 uint32_t depth_texture_width_, depth_texture_height_;
00259 Ogre::PixelBox depth_pixel_box_;
00260
00261 uint32_t uid_counter_;
00262
00263 Ogre::Rectangle2D* highlight_rectangle_;
00264 Ogre::SceneNode* highlight_node_;
00265 Ogre::Camera *camera_;
00266
00267 V_CollObject pixel_buffer_;
00268
00269 bool interaction_enabled_;
00270
00271 bool debug_mode_;
00272
00273 Ogre::MaterialPtr fallback_pick_material_;
00274 Ogre::Technique *fallback_pick_technique_;
00275 Ogre::Technique *fallback_black_technique_;
00276 Ogre::Technique *fallback_depth_technique_;
00277 Ogre::Technique *fallback_pick_cull_technique_;
00278 Ogre::Technique *fallback_black_cull_technique_;
00279 Ogre::Technique *fallback_depth_cull_technique_;
00280
00281 uint32_t texture_size_;
00282
00283 PropertyTreeModel* property_model_;
00284
00285 typedef std::map<std::string, ros::Publisher> PublisherMap;
00286 PublisherMap debug_publishers_;
00287 };
00288
00289 }
00290
00291 #endif // RVIZ_SELECTION_MANAGER_H