$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 #include "visualization_manager.h" 00031 #include "selection/selection_manager.h" 00032 #include "plugin/plugin_manager.h" 00033 #include "plugin/display_type_info.h" 00034 #include "render_panel.h" 00035 #include "displays_panel.h" 00036 #include "viewport_mouse_event.h" 00037 #include "frame_manager.h" 00038 00039 #include "view_controller.h" 00040 #include "view_controllers/xy_orbit_view_controller.h" 00041 #include "view_controllers/orbit_view_controller.h" 00042 #include "view_controllers/fps_view_controller.h" 00043 #include "view_controllers/fixed_orientation_ortho_view_controller.h" 00044 00045 #include "display.h" 00046 #include "display_wrapper.h" 00047 #include "properties/property_manager.h" 00048 #include "properties/property.h" 00049 #include "new_display_dialog.h" 00050 00051 #include "tools/tool.h" 00052 #include "tools/move_tool.h" 00053 #include "tools/goal_tool.h" 00054 #include "tools/initial_pose_tool.h" 00055 #include "tools/selection_tool.h" 00056 #include "tools/interaction_tool.h" 00057 00058 #include <ogre_tools/wx_ogre_render_window.h> 00059 00060 #include <tf/transform_listener.h> 00061 00062 #include <ros/package.h> 00063 00064 #include <wx/timer.h> 00065 #include <wx/propgrid/propgrid.h> 00066 #include <wx/confbase.h> 00067 00068 #include <boost/bind.hpp> 00069 00070 #include <OGRE/OgreRoot.h> 00071 #include <OGRE/OgreSceneManager.h> 00072 #include <OGRE/OgreSceneNode.h> 00073 #include <OGRE/OgreLight.h> 00074 #include <OGRE/OgreViewport.h> 00075 #include <OGRE/OgreMaterialManager.h> 00076 #include <OGRE/OgreMaterial.h> 00077 #include <OGRE/OgreRenderWindow.h> 00078 00079 #include <algorithm> 00080 00081 //include deprecated header, supress compiler warning 00082 #define RVIZ_COMMON_H_NOWARN 00083 #include "common.h" 00084 #undef RVIZ_COMMON_H_NOWARN 00085 00086 00087 namespace rviz 00088 { 00089 00090 VisualizationManager::VisualizationManager( RenderPanel* render_panel, WindowManagerInterface* wm ) 00091 : ogre_root_( Ogre::Root::getSingletonPtr() ) 00092 , update_timer_(0) 00093 , shutting_down_(false) 00094 , current_tool_( NULL ) 00095 , render_panel_( render_panel ) 00096 , time_update_timer_(0.0f) 00097 , frame_update_timer_(0.0f) 00098 , view_controller_(0) 00099 , render_requested_(1) 00100 , frame_count_(0) 00101 , window_manager_(wm) 00102 , disable_update_(false) 00103 , target_frame_is_fixed_frame_(false) 00104 { 00105 initializeCommon(); 00106 00107 frame_manager_ = FrameManager::instance(); 00108 00109 render_panel->setAutoRender(false); 00110 00111 threaded_nh_.setCallbackQueue(&threaded_queue_); 00112 00113 scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC ); 00114 00115 target_scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 00116 00117 Ogre::Matrix3 g_ogre_to_robot_matrix; 00118 g_ogre_to_robot_matrix.FromEulerAnglesYXZ( Ogre::Degree( -90 ), Ogre::Degree( 0 ), Ogre::Degree( -90 ) ); 00119 target_scene_node_->setOrientation( g_ogre_to_robot_matrix ); 00120 00121 Ogre::Light* directional_light = scene_manager_->createLight( "MainDirectional" ); 00122 directional_light->setType( Ogre::Light::LT_DIRECTIONAL ); 00123 directional_light->setDirection( Ogre::Vector3( -1, 0, -1 ) ); 00124 directional_light->setDiffuseColour( Ogre::ColourValue( 1.0f, 1.0f, 1.0f ) ); 00125 00126 property_manager_ = new PropertyManager(); 00127 tool_property_manager_ = new PropertyManager(); 00128 00129 CategoryPropertyWPtr options_category = property_manager_->createCategory( ".Global Options", "", CategoryPropertyWPtr(), this ); 00130 target_frame_property_ = property_manager_->createProperty<TFFrameProperty>( "Target Frame", "", boost::bind( &VisualizationManager::getTargetFrame, this ), 00131 boost::bind( &VisualizationManager::setTargetFrame, this, _1 ), options_category, this ); 00132 setPropertyHelpText(target_frame_property_, "Reference frame for the 3D camera view."); 00133 fixed_frame_property_ = property_manager_->createProperty<EditEnumProperty>( "Fixed Frame", "", boost::bind( &VisualizationManager::getFixedFrame, this ), 00134 boost::bind( &VisualizationManager::setFixedFrame, this, _1 ), options_category, this ); 00135 setPropertyHelpText(fixed_frame_property_, "Frame into which all data is transformed before being displayed."); 00136 background_color_property_ = property_manager_->createProperty<ColorProperty>( "Background Color", "", boost::bind( &VisualizationManager::getBackgroundColor, this ), 00137 boost::bind( &VisualizationManager::setBackgroundColor, this, _1 ), options_category, this ); 00138 setPropertyHelpText(background_color_property_, "Background color for the 3D view."); 00139 status_property_ = property_manager_->createStatus(".Global Status", "", CategoryPropertyWPtr(), this); 00140 00141 CategoryPropertyPtr cat_prop = options_category.lock(); 00142 cat_prop->collapse(); 00143 00144 setBackgroundColor(Color(0.0f, 0.0f, 0.0f)); 00145 00146 Ogre::ResourceGroupManager::getSingleton().createResourceGroup(ROS_PACKAGE_NAME ); 00147 00148 createColorMaterials(); 00149 00150 selection_manager_ = new SelectionManager(this); 00151 00152 threaded_queue_threads_.create_thread(boost::bind(&VisualizationManager::threadedQueueThreadFunc, this)); 00153 00154 plugin_manager_ = new PluginManager; 00155 std::string rviz_path = ros::package::getPath("rviz"); 00156 plugin_manager_->loadDescription(rviz_path + "/lib/default_plugin.yaml"); 00157 plugin_manager_->loadDescriptions(); 00158 00159 { 00160 const L_Plugin& plugins = plugin_manager_->getPlugins(); 00161 L_Plugin::const_iterator it = plugins.begin(); 00162 L_Plugin::const_iterator end = plugins.end(); 00163 for (; it != end; ++it) 00164 { 00165 const PluginPtr& plugin = *it; 00166 plugin->getUnloadingSignal().connect(boost::bind(&VisualizationManager::onPluginUnloading, this, _1)); 00167 } 00168 } 00169 } 00170 00171 VisualizationManager::~VisualizationManager() 00172 { 00173 if (update_timer_) 00174 { 00175 Disconnect( wxEVT_TIMER, update_timer_->GetId(), wxTimerEventHandler( VisualizationManager::onUpdate ), NULL, this ); 00176 update_timer_->Stop(); 00177 delete update_timer_; 00178 } 00179 00180 shutting_down_ = true; 00181 threaded_queue_threads_.join_all(); 00182 00183 if (selection_manager_) 00184 { 00185 selection_manager_->setSelection(M_Picked()); 00186 } 00187 00188 V_DisplayWrapper::iterator it = displays_.begin(); 00189 V_DisplayWrapper::iterator end = displays_.end(); 00190 for (; it != end; ++it) 00191 { 00192 delete *it; 00193 } 00194 displays_.clear(); 00195 00196 V_Tool::iterator tool_it = tools_.begin(); 00197 V_Tool::iterator tool_end = tools_.end(); 00198 for ( ; tool_it != tool_end; ++tool_it ) 00199 { 00200 delete *tool_it; 00201 } 00202 tools_.clear(); 00203 00204 delete plugin_manager_; 00205 delete property_manager_; 00206 delete tool_property_manager_; 00207 00208 delete selection_manager_; 00209 00210 scene_manager_->destroySceneNode( target_scene_node_ ); 00211 00212 if (ogre_root_) 00213 { 00214 ogre_root_->destroySceneManager( scene_manager_ ); 00215 } 00216 } 00217 00218 void VisualizationManager::initialize(const StatusCallback& cb, bool verbose) 00219 { 00220 if (cb) 00221 { 00222 cb("Initializing TF"); 00223 } 00224 00225 setFixedFrame("/map"); 00226 setTargetFrame(FIXED_FRAME_STRING); 00227 00228 render_panel_->getCamera()->setPosition(0, 10, 15); 00229 render_panel_->getCamera()->setNearClipDistance(0.01f); 00230 render_panel_->getCamera()->lookAt(0, 0, 0); 00231 00232 addViewController(XYOrbitViewController::getClassNameStatic(), "XYOrbit"); 00233 addViewController(OrbitViewController::getClassNameStatic(), "Orbit"); 00234 addViewController(FPSViewController::getClassNameStatic(), "FPS"); 00235 addViewController(FixedOrientationOrthoViewController::getClassNameStatic(), "TopDownOrtho"); 00236 setCurrentViewControllerType(OrbitViewController::getClassNameStatic()); 00237 00238 MoveTool *move_tool = createTool< MoveTool >( "Move Camera", 'm' ); 00239 setCurrentTool( move_tool ); 00240 setDefaultTool( move_tool ); 00241 00242 createTool< InteractionTool >( "Interact", 'i' ); 00243 00244 createTool< SelectionTool >( "Select", 's' ); 00245 createTool< GoalTool >( "2D Nav Goal", 'g' ); 00246 createTool< InitialPoseTool >( "2D Pose Estimate", 'p' ); 00247 00248 selection_manager_->initialize( verbose ); 00249 00250 last_update_ros_time_ = ros::Time::now(); 00251 last_update_wall_time_ = ros::WallTime::now(); 00252 } 00253 00254 void VisualizationManager::startUpdate() 00255 { 00256 update_timer_ = new wxTimer( this ); 00257 update_timer_->Start( 33 ); 00258 Connect( update_timer_->GetId(), wxEVT_TIMER, wxTimerEventHandler( VisualizationManager::onUpdate ), NULL, this ); 00259 00260 wxTheApp->Connect(wxID_ANY, wxEVT_IDLE, wxIdleEventHandler(VisualizationManager::onIdle), NULL, this); 00261 } 00262 00263 void createColorMaterial(const std::string& name, const Ogre::ColourValue& color) 00264 { 00265 Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( name, ROS_PACKAGE_NAME ); 00266 mat->setAmbient(color * 0.5f); 00267 mat->setDiffuse(color); 00268 mat->setSelfIllumination(color); 00269 mat->setLightingEnabled(true); 00270 mat->setReceiveShadows(false); 00271 } 00272 00273 void VisualizationManager::createColorMaterials() 00274 { 00275 createColorMaterial("RVIZ/Red", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f)); 00276 createColorMaterial("RVIZ/Green", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f)); 00277 createColorMaterial("RVIZ/Blue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f)); 00278 createColorMaterial("RVIZ/Cyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f)); 00279 } 00280 00281 void VisualizationManager::getDisplayNames(S_string& displays) 00282 { 00283 V_DisplayWrapper::iterator vis_it = displays_.begin(); 00284 V_DisplayWrapper::iterator vis_end = displays_.end(); 00285 for ( ; vis_it != vis_end; ++vis_it ) 00286 { 00287 displays.insert((*vis_it)->getName()); 00288 } 00289 } 00290 00291 std::string VisualizationManager::getTargetFrame() 00292 { 00293 if (target_frame_is_fixed_frame_) 00294 { 00295 return FIXED_FRAME_STRING; 00296 } 00297 00298 return target_frame_; 00299 } 00300 00301 void VisualizationManager::queueRender() 00302 { 00303 if (!render_requested_) 00304 { 00305 wxWakeUpIdle(); 00306 } 00307 00308 render_requested_ = 1; 00309 } 00310 00311 void VisualizationManager::onUpdate( wxTimerEvent& event ) 00312 { 00313 if (disable_update_) 00314 { 00315 return; 00316 } 00317 00318 // make sure that onIdle gets called, even if the application 00319 // does not receive any wake-up events 00320 wxWakeUpIdle(); 00321 } 00322 00323 void VisualizationManager::onIdle(wxIdleEvent& evt) 00324 { 00325 //process pending mouse events 00326 00327 std::deque<ViewportMouseEvent> event_queue; 00328 { 00329 boost::mutex::scoped_lock lock(vme_queue_mutex_); 00330 event_queue.swap( vme_queue_ ); 00331 } 00332 00333 std::deque<ViewportMouseEvent>::iterator event_it; 00334 00335 for ( event_it= event_queue.begin(); event_it!=event_queue.end(); event_it++ ) 00336 { 00337 ViewportMouseEvent &vme = *event_it; 00338 int flags = getCurrentTool()->processMouseEvent(vme); 00339 00340 if ( flags & Tool::Render ) 00341 { 00342 queueRender(); 00343 } 00344 00345 if ( flags & Tool::Finished ) 00346 { 00347 setCurrentTool( getDefaultTool() ); 00348 } 00349 } 00350 00351 00352 ros::WallTime update_start = ros::WallTime::now(); 00353 00354 ros::WallDuration wall_diff = ros::WallTime::now() - last_update_wall_time_; 00355 ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_; 00356 float wall_dt = wall_diff.toSec(); 00357 float ros_dt = ros_diff.toSec(); 00358 00359 if (ros_dt < 0.0) 00360 { 00361 resetTime(); 00362 } 00363 00364 frame_manager_->update(); 00365 00366 ros::spinOnce(); 00367 00368 last_update_ros_time_ = ros::Time::now(); 00369 last_update_wall_time_ = ros::WallTime::now(); 00370 00371 V_DisplayWrapper::iterator vis_it = displays_.begin(); 00372 V_DisplayWrapper::iterator vis_end = displays_.end(); 00373 for ( ; vis_it != vis_end; ++vis_it ) 00374 { 00375 Display* display = (*vis_it)->getDisplay(); 00376 00377 if ( display && display->isEnabled() ) 00378 { 00379 display->update( wall_dt, ros_dt ); 00380 } 00381 } 00382 00383 view_controller_->update(wall_dt, ros_dt); 00384 00385 time_update_timer_ += wall_dt; 00386 00387 if ( time_update_timer_ > 0.1f ) 00388 { 00389 time_update_timer_ = 0.0f; 00390 00391 updateTime(); 00392 } 00393 00394 frame_update_timer_ += wall_dt; 00395 00396 if (frame_update_timer_ > 1.0f) 00397 { 00398 frame_update_timer_ = 0.0f; 00399 00400 updateFrames(); 00401 } 00402 00403 selection_manager_->update(); 00404 00405 if (frame_count_ % 6 == 0) 00406 { 00407 property_manager_->update(); 00408 tool_property_manager_->update(); 00409 } 00410 00411 current_tool_->update(wall_dt, ros_dt); 00412 00413 ros::WallTime cur = ros::WallTime::now(); 00414 double dt = (cur - last_render_).toSec(); 00415 00416 if (dt > 0.1f) 00417 { 00418 render_requested_ = 1; 00419 } 00420 00421 // Cap at 60fps 00422 if (render_requested_ && dt > 0.016f) 00423 { 00424 render_requested_ = 0; 00425 last_render_ = cur; 00426 frame_count_++; 00427 00428 boost::mutex::scoped_lock lock(render_mutex_); 00429 00430 // ros::WallTime start = ros::WallTime::now(); 00431 ogre_root_->renderOneFrame(); 00432 // ros::WallTime end = ros::WallTime::now(); 00433 // ros::WallDuration d = end - start; 00434 // ROS_INFO("Render took [%f] msec", d.toSec() * 1000.0f); 00435 } 00436 00437 evt.Skip(); 00438 } 00439 00440 void VisualizationManager::updateTime() 00441 { 00442 if ( ros_time_begin_.isZero() ) 00443 { 00444 ros_time_begin_ = ros::Time::now(); 00445 } 00446 00447 ros_time_elapsed_ = ros::Time::now() - ros_time_begin_; 00448 00449 if ( wall_clock_begin_.isZero() ) 00450 { 00451 wall_clock_begin_ = ros::WallTime::now(); 00452 } 00453 00454 wall_clock_elapsed_ = ros::WallTime::now() - wall_clock_begin_; 00455 00456 time_changed_(); 00457 } 00458 00459 void VisualizationManager::updateFrames() 00460 { 00461 typedef std::vector<std::string> V_string; 00462 V_string frames; 00463 frame_manager_->getTFClient()->getFrameStrings( frames ); 00464 std::sort(frames.begin(), frames.end()); 00465 00466 EditEnumPropertyPtr target_prop = target_frame_property_.lock(); 00467 EditEnumPropertyPtr fixed_prop = fixed_frame_property_.lock(); 00468 StatusPropertyPtr status_prop = status_property_.lock(); 00469 ROS_ASSERT(target_prop); 00470 ROS_ASSERT(fixed_prop); 00471 ROS_ASSERT(status_prop); 00472 00473 if (frames != available_frames_) 00474 { 00475 fixed_prop->clear(); 00476 00477 V_string::iterator it = frames.begin(); 00478 V_string::iterator end = frames.end(); 00479 for (; it != end; ++it) 00480 { 00481 const std::string& frame = *it; 00482 00483 if (frame.empty()) 00484 { 00485 continue; 00486 } 00487 00488 fixed_prop->addOption(frame); 00489 } 00490 00491 available_frames_ = frames; 00492 00493 frames_changed_(frames); 00494 } 00495 00496 // Check the fixed frame to see if it's ok 00497 std::string error; 00498 if (frame_manager_->frameHasProblems(fixed_frame_, ros::Time(), error)) 00499 { 00500 if (frames.empty()) 00501 { 00502 fixed_prop->setToWarn(); 00503 std::stringstream ss; 00504 ss << "No tf data. Actual error: " << error; 00505 status_prop->setStatus(status_levels::Warn, "Fixed Frame", ss.str()); 00506 } 00507 else 00508 { 00509 fixed_prop->setToError(); 00510 status_prop->setStatus(status_levels::Error, "Fixed Frame", error); 00511 } 00512 } 00513 else 00514 { 00515 fixed_prop->setToOK(); 00516 status_prop->setStatus(status_levels::Ok, "Fixed Frame", "OK"); 00517 } 00518 00519 if (frame_manager_->transformHasProblems(target_frame_, ros::Time(), error)) 00520 { 00521 target_prop->setToError(); 00522 status_prop->setStatus(status_levels::Error, "Target Frame", error); 00523 } 00524 else 00525 { 00526 target_prop->setToOK(); 00527 status_prop->setStatus(status_levels::Ok, "Target Frame", "OK"); 00528 } 00529 } 00530 00531 tf::TransformListener* VisualizationManager::getTFClient() 00532 { 00533 return frame_manager_->getTFClient(); 00534 } 00535 00536 void VisualizationManager::resetTime() 00537 { 00538 resetDisplays(); 00539 frame_manager_->getTFClient()->clear(); 00540 00541 ros_time_begin_ = ros::Time(); 00542 wall_clock_begin_ = ros::WallTime(); 00543 00544 queueRender(); 00545 } 00546 00547 void VisualizationManager::onDisplayCreated(DisplayWrapper* wrapper) 00548 { 00549 Display* display = wrapper->getDisplay(); 00550 display->setRenderCallback( boost::bind( &VisualizationManager::queueRender, this ) ); 00551 display->setLockRenderCallback( boost::bind( &VisualizationManager::lockRender, this ) ); 00552 display->setUnlockRenderCallback( boost::bind( &VisualizationManager::unlockRender, this ) ); 00553 00554 display->setTargetFrame( target_frame_ ); 00555 display->setFixedFrame( fixed_frame_ ); 00556 } 00557 00558 bool VisualizationManager::addDisplay(DisplayWrapper* wrapper, bool enabled) 00559 { 00560 if (getDisplayWrapper(wrapper->getName())) 00561 { 00562 ROS_ERROR("Display of name [%s] already exists", wrapper->getName().c_str()); 00563 return false; 00564 } 00565 00566 display_adding_(wrapper); 00567 displays_.push_back(wrapper); 00568 00569 wrapper->getDisplayCreatedSignal().connect(boost::bind(&VisualizationManager::onDisplayCreated, this, _1)); 00570 wrapper->setPropertyManager( property_manager_, CategoryPropertyWPtr() ); 00571 wrapper->createDisplay(); 00572 00573 display_added_(wrapper); 00574 00575 wrapper->setEnabled(enabled); 00576 00577 return true; 00578 } 00579 00580 void VisualizationManager::removeDisplay( DisplayWrapper* display ) 00581 { 00582 V_DisplayWrapper::iterator it = std::find(displays_.begin(), displays_.end(), display); 00583 ROS_ASSERT( it != displays_.end() ); 00584 00585 display_removing_(display); 00586 00587 displays_.erase( it ); 00588 00589 display_removed_(display); 00590 00591 delete display; 00592 00593 queueRender(); 00594 } 00595 00596 void VisualizationManager::removeAllDisplays() 00597 { 00598 displays_removing_(displays_); 00599 00600 while (!displays_.empty()) 00601 { 00602 removeDisplay(displays_.back()); 00603 } 00604 00605 displays_removed_(V_DisplayWrapper()); 00606 } 00607 00608 void VisualizationManager::removeDisplay( const std::string& name ) 00609 { 00610 DisplayWrapper* display = getDisplayWrapper( name ); 00611 00612 if ( !display ) 00613 { 00614 return; 00615 } 00616 00617 removeDisplay( display ); 00618 } 00619 00620 void VisualizationManager::resetDisplays() 00621 { 00622 V_DisplayWrapper::iterator vis_it = displays_.begin(); 00623 V_DisplayWrapper::iterator vis_end = displays_.end(); 00624 for ( ; vis_it != vis_end; ++vis_it ) 00625 { 00626 Display* display = (*vis_it)->getDisplay(); 00627 00628 if (display) 00629 { 00630 display->reset(); 00631 } 00632 } 00633 } 00634 00635 void VisualizationManager::addTool( Tool* tool ) 00636 { 00637 tools_.push_back( tool ); 00638 00639 tool_added_(tool); 00640 } 00641 00642 void VisualizationManager::setCurrentTool( Tool* tool ) 00643 { 00644 if ( current_tool_ ) 00645 { 00646 current_tool_->deactivate(); 00647 } 00648 00649 current_tool_ = tool; 00650 current_tool_->activate(); 00651 00652 tool_changed_(tool); 00653 } 00654 00655 void VisualizationManager::setDefaultTool( Tool* tool ) 00656 { 00657 default_tool_ = tool; 00658 } 00659 00660 Tool* VisualizationManager::getTool( int index ) 00661 { 00662 ROS_ASSERT( index >= 0 ); 00663 ROS_ASSERT( index < (int)tools_.size() ); 00664 00665 return tools_[ index ]; 00666 } 00667 00668 DisplayWrapper* VisualizationManager::getDisplayWrapper( const std::string& name ) 00669 { 00670 V_DisplayWrapper::iterator vis_it = displays_.begin(); 00671 V_DisplayWrapper::iterator vis_end = displays_.end(); 00672 for ( ; vis_it != vis_end; ++vis_it ) 00673 { 00674 DisplayWrapper* wrapper = *vis_it; 00675 if ( wrapper->getName() == name ) 00676 { 00677 return wrapper; 00678 } 00679 } 00680 00681 return 0; 00682 } 00683 00684 DisplayWrapper* VisualizationManager::getDisplayWrapper( Display* display ) 00685 { 00686 V_DisplayWrapper::iterator vis_it = displays_.begin(); 00687 V_DisplayWrapper::iterator vis_end = displays_.end(); 00688 for ( ; vis_it != vis_end; ++vis_it ) 00689 { 00690 DisplayWrapper* wrapper = *vis_it; 00691 if ( wrapper->getDisplay() == display ) 00692 { 00693 return wrapper; 00694 } 00695 } 00696 00697 return 0; 00698 } 00699 00700 #define CAMERA_TYPE wxT("Camera Type") 00701 #define CAMERA_CONFIG wxT("Camera Config") 00702 00703 void VisualizationManager::loadGeneralConfig( const boost::shared_ptr<wxConfigBase>& config, const StatusCallback& cb ) 00704 { 00705 // Legacy... read camera config from the general config (camera config is now saved in the display config). 00707 wxString camera_type; 00708 if (config->Read(CAMERA_TYPE, &camera_type)) 00709 { 00710 if (setCurrentViewControllerType((const char*)camera_type.char_str())) 00711 { 00712 wxString camera_config; 00713 if (config->Read(CAMERA_CONFIG, &camera_config)) 00714 { 00715 view_controller_->fromString((const char*)camera_config.char_str()); 00716 } 00717 } 00718 } 00719 00720 if (cb) 00721 { 00722 cb("Loading plugins"); 00723 } 00724 00725 plugin_manager_->loadConfig(config); 00726 00727 general_config_loaded_(config); 00728 } 00729 00730 void VisualizationManager::saveGeneralConfig( const boost::shared_ptr<wxConfigBase>& config ) 00731 { 00732 plugin_manager_->saveConfig(config); 00733 general_config_saving_(config); 00734 } 00735 00736 void VisualizationManager::loadDisplayConfig( const boost::shared_ptr<wxConfigBase>& config, const StatusCallback& cb ) 00737 { 00738 disable_update_ = true; 00739 00740 if (cb) 00741 { 00742 cb("Creating displays"); 00743 } 00744 00745 int i = 0; 00746 while (1) 00747 { 00748 wxString name, package, class_name, type; 00749 name.Printf( wxT("Display%d/Name"), i ); 00750 package.Printf( wxT("Display%d/Package"), i ); 00751 class_name.Printf( wxT("Display%d/ClassName"), i ); 00752 type.Printf(wxT("Display%d/Type"), i); 00753 00754 wxString vis_name, vis_package, vis_class, vis_type; 00755 if (!config->Read(name, &vis_name)) 00756 { 00757 break; 00758 } 00759 00760 if (!config->Read(type, &vis_type)) 00761 { 00762 if (!config->Read(package, &vis_package)) 00763 { 00764 break; 00765 } 00766 00767 if (!config->Read(class_name, &vis_class)) 00768 { 00769 break; 00770 } 00771 } 00772 00773 // Legacy support, for loading old config files 00774 if (!vis_type.IsEmpty()) 00775 { 00776 std::string type = (const char*)vis_type.char_str(); 00777 PluginPtr plugin = plugin_manager_->getPluginByDisplayName(type); 00778 if (plugin) 00779 { 00780 const DisplayTypeInfoPtr& info = plugin->getDisplayTypeInfoByDisplayName(type); 00781 createDisplay(plugin->getPackageName(), info->class_name, (const char*)vis_name.char_str(), false); 00782 } 00783 else 00784 { 00785 ROS_WARN("Display type [%s] no longer exists for display [%s]", type.c_str(), (const char*)vis_name.char_str()); 00786 } 00787 } 00788 else 00789 { 00790 createDisplay((const char*)vis_package.char_str(), (const char*)vis_class.char_str(), (const char*)vis_name.char_str(), false); 00791 } 00792 00793 ++i; 00794 } 00795 00796 property_manager_->load( config, cb ); 00797 tool_property_manager_->load( config, cb ); 00798 00799 wxString camera_type; 00800 if (config->Read(CAMERA_TYPE, &camera_type)) 00801 { 00802 if (setCurrentViewControllerType((const char*)camera_type.char_str())) 00803 { 00804 wxString camera_config; 00805 if (config->Read(CAMERA_CONFIG, &camera_config)) 00806 { 00807 view_controller_->fromString((const char*)camera_config.char_str()); 00808 } 00809 } 00810 } 00811 00812 displays_config_loaded_(config); 00813 00814 disable_update_ = false; 00815 } 00816 00817 void VisualizationManager::saveDisplayConfig( const boost::shared_ptr<wxConfigBase>& config ) 00818 { 00819 int i = 0; 00820 V_DisplayWrapper::iterator vis_it = displays_.begin(); 00821 V_DisplayWrapper::iterator vis_end = displays_.end(); 00822 for ( ; vis_it != vis_end; ++vis_it, ++i ) 00823 { 00824 DisplayWrapper* wrapper = *vis_it; 00825 00826 wxString name, package, class_name; 00827 name.Printf( wxT("Display%d/Name"), i ); 00828 package.Printf( wxT("Display%d/Package"), i ); 00829 class_name.Printf( wxT("Display%d/ClassName"), i ); 00830 config->Write( name, wxString::FromAscii( wrapper->getName().c_str() ) ); 00831 config->Write( package, wxString::FromAscii( wrapper->getPackage().c_str() ) ); 00832 config->Write( class_name, wxString::FromAscii( wrapper->getClassName().c_str() ) ); 00833 } 00834 00835 property_manager_->save( config ); 00836 tool_property_manager_->save( config ); 00837 00838 if (view_controller_) 00839 { 00840 config->Write(CAMERA_TYPE, wxString::FromAscii(view_controller_->getClassName().c_str())); 00841 config->Write(CAMERA_CONFIG, wxString::FromAscii(view_controller_->toString().c_str())); 00842 } 00843 00844 displays_config_saving_(config); 00845 } 00846 00847 DisplayWrapper* VisualizationManager::createDisplay( const std::string& package, const std::string& class_name, const std::string& name, bool enabled ) 00848 { 00849 PluginPtr plugin = plugin_manager_->getPluginByPackage(package); 00850 if (!plugin) 00851 { 00852 ROS_ERROR("Package [%s] does not have any plugins loaded available, for display of type [%s], and name [%s]", package.c_str(), class_name.c_str(), name.c_str()); 00853 } 00854 00855 DisplayWrapper* wrapper = new DisplayWrapper(package, class_name, plugin, name, this); 00856 if (addDisplay(wrapper, enabled)) 00857 { 00858 return wrapper; 00859 } 00860 else 00861 { 00862 delete wrapper; 00863 return 0; 00864 } 00865 } 00866 00867 void VisualizationManager::setTargetFrame( const std::string& _frame ) 00868 { 00869 target_frame_is_fixed_frame_ = false; 00870 std::string frame = _frame; 00871 if (frame == FIXED_FRAME_STRING) 00872 { 00873 frame = fixed_frame_; 00874 target_frame_is_fixed_frame_ = true; 00875 } 00876 00877 std::string remapped_name = frame_manager_->getTFClient()->resolve(frame); 00878 00879 if (target_frame_ == remapped_name) 00880 { 00881 return; 00882 } 00883 00884 target_frame_ = remapped_name; 00885 00886 V_DisplayWrapper::iterator it = displays_.begin(); 00887 V_DisplayWrapper::iterator end = displays_.end(); 00888 for ( ; it != end; ++it ) 00889 { 00890 Display* display = (*it)->getDisplay(); 00891 00892 if (display) 00893 { 00894 display->setTargetFrame(target_frame_); 00895 } 00896 } 00897 00898 propertyChanged(target_frame_property_); 00899 00900 if (view_controller_) 00901 { 00902 view_controller_->setTargetFrame(target_frame_); 00903 } 00904 } 00905 00906 void VisualizationManager::setFixedFrame( const std::string& frame ) 00907 { 00908 std::string remapped_name = frame_manager_->getTFClient()->resolve(frame); 00909 00910 if (fixed_frame_ == remapped_name) 00911 { 00912 return; 00913 } 00914 00915 fixed_frame_ = remapped_name; 00916 00917 frame_manager_->setFixedFrame(fixed_frame_); 00918 00919 V_DisplayWrapper::iterator it = displays_.begin(); 00920 V_DisplayWrapper::iterator end = displays_.end(); 00921 for ( ; it != end; ++it ) 00922 { 00923 Display* display = (*it)->getDisplay(); 00924 00925 if (display) 00926 { 00927 display->setFixedFrame(fixed_frame_); 00928 } 00929 } 00930 00931 propertyChanged(fixed_frame_property_); 00932 00933 if (target_frame_is_fixed_frame_) 00934 { 00935 setTargetFrame(FIXED_FRAME_STRING); 00936 } 00937 } 00938 00939 bool VisualizationManager::isValidDisplay(const DisplayWrapper* display) 00940 { 00941 V_DisplayWrapper::iterator it = displays_.begin(); 00942 V_DisplayWrapper::iterator end = displays_.end(); 00943 for ( ; it != end; ++it ) 00944 { 00945 if (display == (*it)) 00946 { 00947 return true; 00948 } 00949 } 00950 00951 return false; 00952 } 00953 00954 double VisualizationManager::getWallClock() 00955 { 00956 return ros::WallTime::now().toSec(); 00957 } 00958 00959 double VisualizationManager::getROSTime() 00960 { 00961 return (ros_time_begin_ + ros_time_elapsed_).toSec(); 00962 } 00963 00964 double VisualizationManager::getWallClockElapsed() 00965 { 00966 return wall_clock_elapsed_.toSec(); 00967 } 00968 00969 double VisualizationManager::getROSTimeElapsed() 00970 { 00971 return ros_time_elapsed_.toSec(); 00972 } 00973 00974 void VisualizationManager::setBackgroundColor(const Color& c) 00975 { 00976 background_color_ = c; 00977 00978 render_panel_->getViewport()->setBackgroundColour(Ogre::ColourValue(c.r_, c.g_, c.b_, 1.0f)); 00979 00980 propertyChanged(background_color_property_); 00981 00982 queueRender(); 00983 } 00984 00985 const Color& VisualizationManager::getBackgroundColor() 00986 { 00987 return background_color_; 00988 } 00989 00990 void VisualizationManager::handleChar( wxKeyEvent& event ) 00991 { 00992 if ( event.GetKeyCode() == WXK_ESCAPE ) 00993 { 00994 setCurrentTool( getDefaultTool() ); 00995 00996 return; 00997 } 00998 00999 char key = event.GetKeyCode(); 01000 V_Tool::iterator it = tools_.begin(); 01001 V_Tool::iterator end = tools_.end(); 01002 for ( ; it != end; ++it ) 01003 { 01004 Tool* tool = *it; 01005 if ( tool->getShortcutKey() == key && tool != getCurrentTool() ) 01006 { 01007 setCurrentTool( tool ); 01008 return; 01009 } 01010 } 01011 01012 getCurrentTool()->processKeyEvent(event); 01013 } 01014 01015 void VisualizationManager::addViewController(const std::string& class_name, const std::string& name) 01016 { 01017 view_controller_type_added_(class_name, name); 01018 } 01019 01020 bool VisualizationManager::setCurrentViewControllerType(const std::string& type) 01021 { 01022 if (view_controller_ && (view_controller_->getClassName() == type || view_controller_->getName() == type)) 01023 { 01024 return true; 01025 } 01026 01027 bool found = true; 01028 // hack hack hack hack until this becomes truly plugin based 01029 if (type == "rviz::OrbitViewController" || type == "Orbit") 01030 { 01031 view_controller_ = new OrbitViewController(this, "Orbit",target_scene_node_); 01032 } 01033 else if (type == "rviz::XYOrbitViewController" || type == "XYOrbit" || 01034 type == "rviz::SimpleOrbitViewController" || type == "SimpleOrbit" /* the old class name */) 01035 { 01036 view_controller_ = new XYOrbitViewController(this, "XYOrbit",target_scene_node_); 01037 } 01038 else if (type == "rviz::FPSViewController" || type == "FPS") 01039 { 01040 view_controller_ = new FPSViewController(this, "FPS",target_scene_node_); 01041 } 01042 else if (type == "rviz::FixedOrientationOrthoViewController" || type == "TopDownOrtho" || type == "Top-down Orthographic") 01043 { 01044 FixedOrientationOrthoViewController* controller = new FixedOrientationOrthoViewController(this, "TopDownOrtho",target_scene_node_); 01045 Ogre::Quaternion orient; 01046 orient.FromAngleAxis(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); 01047 controller->setOrientation(orient); 01048 view_controller_ = controller; 01049 } 01050 else if (!view_controller_) 01051 { 01052 view_controller_ = new OrbitViewController(this, "Orbit",target_scene_node_); 01053 } 01054 else 01055 { 01056 found = false; 01057 } 01058 01059 if (found) 01060 { 01061 view_controller_->setTargetFrame( target_frame_ ); 01062 render_panel_->setViewController(view_controller_); 01063 view_controller_type_changed_(view_controller_); 01064 } 01065 01066 return found; 01067 } 01068 01069 std::string VisualizationManager::getCurrentViewControllerType() 01070 { 01071 return view_controller_->getClassName(); 01072 } 01073 01074 void VisualizationManager::handleMouseEvent(ViewportMouseEvent& vme) 01075 { 01076 boost::mutex::scoped_lock lock( vme_queue_mutex_ ); 01077 vme_queue_.push_back(vme); 01078 } 01079 01080 void VisualizationManager::threadedQueueThreadFunc() 01081 { 01082 while (!shutting_down_) 01083 { 01084 threaded_queue_.callOne(ros::WallDuration(0.1)); 01085 } 01086 } 01087 01088 void VisualizationManager::onPluginUnloading(const PluginStatus& status) 01089 { 01090 // We need to force an update of the property manager here, because the memory allocated for properties is done inside their shared objects. 01091 // If a plugin is unloaded and then later the update is called, the weak pointers can cause crashes, because the objects they point to are 01092 // no longer valid. 01093 property_manager_->update(); 01094 } 01095 01096 } // namespace rviz