$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 "helpers/color.h" 00035 #include "properties/forwards.h" 00036 00037 #include <wx/event.h> 00038 #include <wx/stopwatch.h> 00039 00040 #include <boost/signal.hpp> 00041 #include <boost/thread.hpp> 00042 00043 #include <vector> 00044 #include <map> 00045 #include <set> 00046 00047 #include <ros/ros.h> 00048 #include <ros/callback_queue.h> 00049 #include <ros/time.h> 00050 00051 namespace Ogre 00052 { 00053 class Root; 00054 class SceneManager; 00055 class SceneNode; 00056 class Camera; 00057 class RaySceneQuery; 00058 } 00059 00060 namespace tf 00061 { 00062 class TransformListener; 00063 } 00064 00065 class wxTimerEvent; 00066 class wxTimer; 00067 class wxPropertyGrid; 00068 class wxPropertyGridEvent; 00069 class wxConfigBase; 00070 class wxKeyEvent; 00071 class wxIdleEvent; 00072 00073 namespace rviz 00074 { 00075 00076 class PropertyManager; 00077 class SelectionManager; 00078 class RenderPanel; 00079 class Display; 00080 class Tool; 00081 class ViewportMouseEvent; 00082 class WindowManagerInterface; 00083 class PluginManager; 00084 class PluginStatus; 00085 class FrameManager; 00086 class ViewController; 00087 typedef boost::shared_ptr<FrameManager> FrameManagerPtr; 00088 00089 class DisplayWrapper; 00090 typedef std::vector<DisplayWrapper*> V_DisplayWrapper; 00091 00092 typedef boost::signal<void (DisplayWrapper*)> DisplayWrapperSignal; 00093 typedef boost::signal<void (const V_DisplayWrapper&)> DisplayWrappersSignal; 00094 typedef boost::signal<void (const V_string&)> FramesChangedSignal; 00095 typedef boost::signal<void (const boost::shared_ptr<wxConfigBase>&)> ConfigSignal; 00096 typedef boost::signal<void (Tool*)> ToolSignal; 00097 typedef boost::signal<void (const std::string&, const std::string&)> ViewControllerTypeAddedSignal; 00098 typedef boost::signal<void (ViewController*)> ViewControllerSignal; 00099 typedef boost::signal<void (void)> TimeSignal; 00100 00101 class DisplayTypeInfo; 00102 typedef boost::shared_ptr<DisplayTypeInfo> DisplayTypeInfoPtr; 00103 00104 class VisualizationManager : public wxEvtHandler 00105 { 00106 public: 00110 VisualizationManager(RenderPanel* render_panel, WindowManagerInterface* wm = 0); 00111 virtual ~VisualizationManager(); 00112 00113 void initialize(const StatusCallback& cb = StatusCallback(), bool verbose=false); 00114 void startUpdate(); 00115 00123 DisplayWrapper* createDisplay( const std::string& package, const std::string& class_name, const std::string& name, bool enabled ); 00124 00129 void removeDisplay( DisplayWrapper* display ); 00134 void removeDisplay( const std::string& name ); 00138 void removeAllDisplays(); 00139 00140 template< class T > 00141 T* createTool( const std::string& name, char shortcut_key ) 00142 { 00143 T* tool = new T( name, shortcut_key, this ); 00144 addTool( tool ); 00145 00146 return tool; 00147 } 00148 00149 void addTool( Tool* tool ); 00150 Tool* getCurrentTool() { return current_tool_; } 00151 Tool* getTool( int index ); 00152 void setCurrentTool( Tool* tool ); 00153 void setDefaultTool( Tool* tool ); 00154 Tool* getDefaultTool() { return default_tool_; } 00155 00160 void loadGeneralConfig( const boost::shared_ptr<wxConfigBase>& config, const StatusCallback& cb = StatusCallback() ); 00165 void saveGeneralConfig( const boost::shared_ptr<wxConfigBase>& config ); 00170 void loadDisplayConfig( const boost::shared_ptr<wxConfigBase>& config, const StatusCallback& cb = StatusCallback() ); 00175 void saveDisplayConfig( const boost::shared_ptr<wxConfigBase>& config ); 00176 00181 void setTargetFrame( const std::string& frame ); 00182 std::string getTargetFrame(); 00183 00188 void setFixedFrame( const std::string& frame ); 00189 const std::string& getFixedFrame() { return fixed_frame_; } 00190 00195 DisplayWrapper* getDisplayWrapper( const std::string& name ); 00196 00201 DisplayWrapper* getDisplayWrapper( Display* display ); 00202 00203 PropertyManager* getPropertyManager() { return property_manager_; } 00204 PropertyManager* getToolPropertyManager() { return tool_property_manager_; } 00205 00206 bool isValidDisplay( const DisplayWrapper* display ); 00207 00208 tf::TransformListener* getTFClient(); 00209 Ogre::SceneManager* getSceneManager() { return scene_manager_; } 00210 00211 RenderPanel* getRenderPanel() { return render_panel_; } 00212 00213 typedef std::set<std::string> S_string; 00214 void getDisplayNames(S_string& displays); 00215 V_DisplayWrapper& getDisplays() { return displays_; } 00216 00217 void resetDisplays(); 00218 00219 double getWallClock(); 00220 double getROSTime(); 00221 double getWallClockElapsed(); 00222 double getROSTimeElapsed(); 00223 00224 void handleChar( wxKeyEvent& event ); 00225 void handleMouseEvent( ViewportMouseEvent& event ); 00226 00227 void setBackgroundColor(const Color& c); 00228 const Color& getBackgroundColor(); 00229 00230 void resetTime(); 00231 00232 ViewController* getCurrentViewController() { return view_controller_; } 00233 std::string getCurrentViewControllerType(); 00234 bool setCurrentViewControllerType(const std::string& type); 00235 00236 SelectionManager* getSelectionManager() { return selection_manager_; } 00237 00238 void lockRender() { render_mutex_.lock(); } 00239 void unlockRender() { render_mutex_.unlock(); } 00244 void queueRender(); 00245 00246 WindowManagerInterface* getWindowManager() { return window_manager_; } 00247 00248 ros::CallbackQueueInterface* getUpdateQueue() { return ros::getGlobalCallbackQueue(); } 00249 ros::CallbackQueueInterface* getThreadedQueue() { return &threaded_queue_; } 00250 00251 PluginManager* getPluginManager() { return plugin_manager_; } 00252 FrameManager* getFrameManager() { return frame_manager_.get(); } 00253 00254 uint64_t getFrameCount() { return frame_count_; } 00255 00256 protected: 00261 bool addDisplay(DisplayWrapper* wrapper, bool enabled); 00262 00263 void addViewController(const std::string& class_name, const std::string& name); 00264 00266 void onUpdate( wxTimerEvent& event ); 00267 void onIdle(wxIdleEvent& event); 00268 00269 void updateRelativeNode(); 00270 00271 void incomingROSTime(); 00272 00273 void updateTime(); 00274 void updateFrames(); 00275 00276 void onDisplayCreated(DisplayWrapper* wrapper); 00277 00278 void createColorMaterials(); 00279 00280 void threadedQueueThreadFunc(); 00281 00282 void onPluginUnloading(const PluginStatus& status); 00283 00284 Ogre::Root* ogre_root_; 00285 Ogre::SceneManager* scene_manager_; 00286 00287 wxTimer* update_timer_; 00288 ros::Time last_update_ros_time_; 00289 ros::WallTime last_update_wall_time_; 00290 00291 ros::CallbackQueue threaded_queue_; 00292 boost::thread_group threaded_queue_threads_; 00293 ros::NodeHandle update_nh_; 00294 ros::NodeHandle threaded_nh_; 00295 volatile bool shutting_down_; 00296 00297 00298 V_DisplayWrapper displays_; 00299 00300 typedef std::vector< Tool* > V_Tool; 00301 V_Tool tools_; 00302 Tool* current_tool_; 00303 Tool* default_tool_; 00304 00305 std::string target_frame_; 00306 std::string fixed_frame_; 00307 00308 PropertyManager* property_manager_; 00309 PropertyManager* tool_property_manager_; 00310 TFFramePropertyWPtr target_frame_property_; 00311 EditEnumPropertyWPtr fixed_frame_property_; 00312 StatusPropertyWPtr status_property_; 00313 00314 V_string available_frames_; 00315 00316 RenderPanel* render_panel_; 00317 00318 ros::WallTime wall_clock_begin_; 00319 ros::Time ros_time_begin_; 00320 ros::WallDuration wall_clock_elapsed_; 00321 ros::Duration ros_time_elapsed_; 00322 00323 Color background_color_; 00324 ColorPropertyWPtr background_color_property_; 00325 00326 float time_update_timer_; 00327 float frame_update_timer_; 00328 00329 ViewController* view_controller_; 00330 00331 SelectionManager* selection_manager_; 00332 00333 boost::mutex render_mutex_; 00334 uint32_t render_requested_; 00335 uint64_t frame_count_; 00336 ros::WallTime last_render_; 00337 00338 WindowManagerInterface* window_manager_; 00339 00340 PluginManager* plugin_manager_; 00341 FrameManagerPtr frame_manager_; 00342 00343 bool disable_update_; 00344 bool target_frame_is_fixed_frame_; 00345 00346 Ogre::SceneNode *target_scene_node_; 00347 00348 std::deque<ViewportMouseEvent> vme_queue_; 00349 boost::mutex vme_queue_mutex_; 00350 00351 public: 00352 FramesChangedSignal& getFramesChangedSignal() { return frames_changed_; } 00353 DisplayWrapperSignal& getDisplayAddingSignal() { return display_adding_; } 00354 DisplayWrapperSignal& getDisplayAddedSignal() { return display_added_; } 00355 DisplayWrapperSignal& getDisplayRemovingSignal() { return display_removing_; } 00356 DisplayWrapperSignal& getDisplayRemovedSignal() { return display_removed_; } 00357 DisplayWrappersSignal& getDisplaysRemovingSignal() { return displays_removing_; } 00358 DisplayWrappersSignal& getDisplaysRemovedSignal() { return displays_removed_; } 00359 ConfigSignal& getDisplaysConfigLoadedSignal() { return displays_config_loaded_; } 00360 ConfigSignal& getDisplaysConfigSavingSignal() { return displays_config_saving_; } 00361 ConfigSignal& getGeneralConfigLoadedSignal() { return general_config_loaded_; } 00362 ConfigSignal& getGeneralConfigSavingSignal() { return general_config_saving_; } 00363 ToolSignal& getToolAddedSignal() { return tool_added_; } 00364 ToolSignal& getToolChangedSignal() { return tool_changed_; } 00365 ViewControllerTypeAddedSignal& getViewControllerTypeAddedSignal() { return view_controller_type_added_; } 00366 ViewControllerSignal& getViewControllerTypeChangedSignal() { return view_controller_type_changed_; } 00367 TimeSignal& getTimeChangedSignal() { return time_changed_; } 00368 00369 private: 00370 FramesChangedSignal frames_changed_; 00371 DisplayWrapperSignal display_adding_; 00372 DisplayWrapperSignal display_added_; 00373 DisplayWrapperSignal display_removing_; 00374 DisplayWrapperSignal display_removed_; 00375 DisplayWrappersSignal displays_removing_; 00376 DisplayWrappersSignal displays_removed_; 00377 ConfigSignal displays_config_loaded_; 00378 ConfigSignal displays_config_saving_; 00379 ConfigSignal general_config_loaded_; 00380 ConfigSignal general_config_saving_; 00381 ToolSignal tool_added_; 00382 ToolSignal tool_changed_; 00383 ViewControllerTypeAddedSignal view_controller_type_added_; 00384 ViewControllerSignal view_controller_type_changed_; 00385 TimeSignal time_changed_; 00386 }; 00387 00388 } 00389 00390 #endif /* RVIZ_VISUALIZATION_MANAGER_H_ */