$search
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 00031 #ifndef RVIZ_VISUALIZATION_MANAGER_H_ 00032 #define RVIZ_VISUALIZATION_MANAGER_H_ 00033 00034 #include <QObject> 00035 #include <QTimer> 00036 00037 #include "rviz/helpers/color.h" 00038 #include "rviz/properties/forwards.h" 00039 00040 #include <boost/thread.hpp> 00041 00042 #include <vector> 00043 #include <map> 00044 #include <set> 00045 00046 #include <ros/ros.h> 00047 #include <ros/callback_queue.h> 00048 #include <ros/time.h> 00049 00050 #include <pluginlib/class_loader.h> 00051 00052 class QKeyEvent; 00053 00054 namespace Ogre 00055 { 00056 class Root; 00057 class SceneManager; 00058 class SceneNode; 00059 } 00060 00061 namespace tf 00062 { 00063 class TransformListener; 00064 } 00065 00066 namespace rviz 00067 { 00068 00069 class Config; 00070 class PropertyManager; 00071 class SelectionManager; 00072 class RenderPanel; 00073 class Display; 00074 class Tool; 00075 class ViewportMouseEvent; 00076 class WindowManagerInterface; 00077 class PluginManager; 00078 class PluginStatus; 00079 class FrameManager; 00080 class ViewController; 00081 typedef boost::shared_ptr<FrameManager> FrameManagerPtr; 00082 00083 class DisplayWrapper; 00084 typedef std::vector<DisplayWrapper*> V_DisplayWrapper; 00085 00086 class DisplayTypeInfo; 00087 typedef boost::shared_ptr<DisplayTypeInfo> DisplayTypeInfoPtr; 00088 00089 class VisualizationManager: public QObject 00090 { 00091 Q_OBJECT 00092 public: 00096 VisualizationManager(RenderPanel* render_panel, WindowManagerInterface* wm = 0); 00097 virtual ~VisualizationManager(); 00098 00099 void initialize(const StatusCallback& cb = StatusCallback(), bool verbose=false); 00100 void startUpdate(); 00101 00109 DisplayWrapper* createDisplay( const std::string& class_lookup_name, const std::string& name, bool enabled ); 00110 00115 void removeDisplay( DisplayWrapper* display ); 00120 void removeDisplay( const std::string& name ); 00124 void removeAllDisplays(); 00125 00126 template< class T > 00127 T* createTool( const std::string& name, char shortcut_key ) 00128 { 00129 T* tool = new T( name, shortcut_key, this ); 00130 addTool( tool ); 00131 00132 return tool; 00133 } 00134 00135 void addTool( Tool* tool ); 00136 Tool* getCurrentTool() { return current_tool_; } 00137 Tool* getTool( int index ); 00138 void setCurrentTool( Tool* tool ); 00139 void setDefaultTool( Tool* tool ); 00140 Tool* getDefaultTool() { return default_tool_; } 00141 00142 // The "general" config file stores window geometry, plugin status, and view controller state. 00143 void loadGeneralConfig( const boost::shared_ptr<Config>& config, const StatusCallback& cb = StatusCallback() ); 00144 void saveGeneralConfig( const boost::shared_ptr<Config>& config ); 00145 00146 // The "display" config file stores the properties of each Display. 00147 void loadDisplayConfig( const boost::shared_ptr<Config>& config, const StatusCallback& cb = StatusCallback() ); 00148 void saveDisplayConfig( const boost::shared_ptr<Config>& config ); 00149 00154 void setTargetFrame( const std::string& frame ); 00155 std::string getTargetFrame(); 00156 00161 void setFixedFrame( const std::string& frame ); 00162 const std::string& getFixedFrame() { return fixed_frame_; } 00163 00168 DisplayWrapper* getDisplayWrapper( const std::string& name ); 00169 00174 DisplayWrapper* getDisplayWrapper( Display* display ); 00175 00176 PropertyManager* getPropertyManager() { return property_manager_; } 00177 PropertyManager* getToolPropertyManager() { return tool_property_manager_; } 00178 00179 bool isValidDisplay( const DisplayWrapper* display ); 00180 00181 tf::TransformListener* getTFClient(); 00182 Ogre::SceneManager* getSceneManager() { return scene_manager_; } 00183 00184 RenderPanel* getRenderPanel() { return render_panel_; } 00185 00186 typedef std::set<std::string> S_string; 00187 void getDisplayNames(S_string& displays); 00188 V_DisplayWrapper& getDisplays() { return displays_; } 00189 00190 void resetDisplays(); 00191 00192 double getWallClock(); 00193 double getROSTime(); 00194 double getWallClockElapsed(); 00195 double getROSTimeElapsed(); 00196 00197 void handleChar( QKeyEvent* event ); 00198 void handleMouseEvent( ViewportMouseEvent& event ); 00199 00200 void setBackgroundColor(const Color& c); 00201 const Color& getBackgroundColor(); 00202 00203 void resetTime(); 00204 00205 ViewController* getCurrentViewController() { return view_controller_; } 00206 std::string getCurrentViewControllerType(); 00207 bool setCurrentViewControllerType(const std::string& type); 00208 00209 SelectionManager* getSelectionManager() { return selection_manager_; } 00210 00211 void lockRender() { render_mutex_.lock(); } 00212 void unlockRender() { render_mutex_.unlock(); } 00217 void queueRender(); 00218 00219 WindowManagerInterface* getWindowManager() { return window_manager_; } 00220 00221 ros::CallbackQueueInterface* getUpdateQueue() { return ros::getGlobalCallbackQueue(); } 00222 ros::CallbackQueueInterface* getThreadedQueue() { return &threaded_queue_; } 00223 00224 pluginlib::ClassLoader<Display>* getDisplayClassLoader() { return display_class_loader_; } 00225 // PluginManager* getPluginManager() { return plugin_manager_; } 00226 FrameManager* getFrameManager() { return frame_manager_.get(); } 00227 00228 uint64_t getFrameCount() { return frame_count_; } 00229 00230 Q_SIGNALS: 00231 void displayAdding( DisplayWrapper* ); 00232 void displayAdded( DisplayWrapper* ); 00233 void displayRemoving( DisplayWrapper* ); 00234 void displayRemoved( DisplayWrapper* ); 00235 void displaysRemoving( const V_DisplayWrapper& ); 00236 void displaysRemoved( const V_DisplayWrapper& ); 00237 void displaysConfigLoaded( const boost::shared_ptr<Config>& ); 00238 void displaysConfigSaved( const boost::shared_ptr<Config>& ); 00239 void generalConfigLoaded( const boost::shared_ptr<Config>& ); 00240 void generalConfigSaving( const boost::shared_ptr<Config>& ); 00241 void toolAdded( Tool* ); 00242 void toolChanged( Tool* ); 00243 void viewControllerTypeAdded( const std::string& class_name, const std::string& name ); 00244 void viewControllerChanged( ViewController* ); 00245 void timeChanged(); 00246 00247 protected Q_SLOTS: 00249 void onUpdate(); 00250 00252 void onIdle(); 00253 00254 void onDisplayCreated( DisplayWrapper* wrapper ); 00255 00256 protected: 00261 bool addDisplay(DisplayWrapper* wrapper, bool enabled); 00262 00263 void addViewController(const std::string& class_name, const std::string& name); 00264 00265 void updateRelativeNode(); 00266 00267 void incomingROSTime(); 00268 00269 void updateTime(); 00270 void updateFrames(); 00271 00272 void createColorMaterials(); 00273 00274 void threadedQueueThreadFunc(); 00275 00276 void onPluginUnloading(const PluginStatus& status); 00277 00278 Ogre::Root* ogre_root_; 00279 Ogre::SceneManager* scene_manager_; 00280 00281 QTimer* update_timer_; 00282 ros::Time last_update_ros_time_; 00283 ros::WallTime last_update_wall_time_; 00284 00285 QTimer* idle_timer_; 00286 00287 ros::CallbackQueue threaded_queue_; 00288 boost::thread_group threaded_queue_threads_; 00289 ros::NodeHandle update_nh_; 00290 ros::NodeHandle threaded_nh_; 00291 volatile bool shutting_down_; 00292 00293 00294 V_DisplayWrapper displays_; 00295 00296 typedef std::vector< Tool* > V_Tool; 00297 V_Tool tools_; 00298 Tool* current_tool_; 00299 Tool* default_tool_; 00300 00301 std::string target_frame_; 00302 std::string fixed_frame_; 00303 00304 PropertyManager* property_manager_; 00305 PropertyManager* tool_property_manager_; 00306 TFFramePropertyWPtr target_frame_property_; 00307 EditEnumPropertyWPtr fixed_frame_property_; 00308 StatusPropertyWPtr status_property_; 00309 00310 V_string available_frames_; 00311 00312 RenderPanel* render_panel_; 00313 00314 ros::WallTime wall_clock_begin_; 00315 ros::Time ros_time_begin_; 00316 ros::WallDuration wall_clock_elapsed_; 00317 ros::Duration ros_time_elapsed_; 00318 00319 Color background_color_; 00320 ColorPropertyWPtr background_color_property_; 00321 00322 float time_update_timer_; 00323 float frame_update_timer_; 00324 00325 ViewController* view_controller_; 00326 00327 SelectionManager* selection_manager_; 00328 00329 boost::mutex render_mutex_; 00330 uint32_t render_requested_; 00331 uint64_t frame_count_; 00332 ros::WallTime last_render_; 00333 00334 WindowManagerInterface* window_manager_; 00335 00336 pluginlib::ClassLoader<Display>* display_class_loader_; 00337 // PluginManager* plugin_manager_; 00338 FrameManagerPtr frame_manager_; 00339 00340 bool disable_update_; 00341 bool target_frame_is_fixed_frame_; 00342 00343 Ogre::SceneNode *target_scene_node_; 00344 00345 std::deque<ViewportMouseEvent> vme_queue_; 00346 boost::mutex vme_queue_mutex_; 00347 }; 00348 00349 } 00350 00351 #endif /* RVIZ_VISUALIZATION_MANAGER_H_ */