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 #include <algorithm>
00031
00032 #include <QApplication>
00033
00034 #include <boost/bind.hpp>
00035
00036 #include <OGRE/OgreRoot.h>
00037 #include <OGRE/OgreSceneManager.h>
00038 #include <OGRE/OgreSceneNode.h>
00039 #include <OGRE/OgreLight.h>
00040 #include <OGRE/OgreViewport.h>
00041 #include <OGRE/OgreMaterialManager.h>
00042 #include <OGRE/OgreMaterial.h>
00043 #include <OGRE/OgreRenderWindow.h>
00044
00045 #include <yaml-cpp/node.h>
00046 #include <yaml-cpp/emitter.h>
00047
00048 #include <tf/transform_listener.h>
00049
00050 #include <ros/package.h>
00051
00052 #include "rviz/display.h"
00053 #include "rviz/display_factory.h"
00054 #include "rviz/display_group.h"
00055 #include "rviz/displays_panel.h"
00056 #include "rviz/frame_manager.h"
00057 #include "rviz/ogre_helpers/qt_ogre_render_window.h"
00058 #include "rviz/properties/color_property.h"
00059 #include "rviz/properties/parse_color.h"
00060 #include "rviz/properties/property.h"
00061 #include "rviz/properties/property_tree_model.h"
00062 #include "rviz/properties/status_list.h"
00063 #include "rviz/properties/tf_frame_property.h"
00064 #include "rviz/render_panel.h"
00065 #include "rviz/selection/selection_manager.h"
00066 #include "rviz/tool.h"
00067 #include "rviz/view_controller.h"
00068 #include "rviz/view_controllers/fixed_orientation_ortho_view_controller.h"
00069 #include "rviz/view_controllers/fps_view_controller.h"
00070 #include "rviz/view_controllers/orbit_view_controller.h"
00071 #include "rviz/view_controllers/xy_orbit_view_controller.h"
00072 #include "rviz/viewport_mouse_event.h"
00073
00074 #include "rviz/visualization_manager.h"
00075
00076 #define CAMERA_TYPE "Camera Type"
00077 #define CAMERA_CONFIG "Camera Config"
00078
00079 namespace rviz
00080 {
00081
00082 VisualizationManager::VisualizationManager( RenderPanel* render_panel, WindowManagerInterface* wm )
00083 : ogre_root_( Ogre::Root::getSingletonPtr() )
00084 , update_timer_(0)
00085 , shutting_down_(false)
00086 , current_tool_( NULL )
00087 , render_panel_( render_panel )
00088 , time_update_timer_(0.0f)
00089 , frame_update_timer_(0.0f)
00090 , view_controller_(0)
00091 , render_requested_(1)
00092 , frame_count_(0)
00093 , window_manager_(wm)
00094 , disable_update_(false)
00095 {
00096 frame_manager_ = new FrameManager();
00097
00098 render_panel->setAutoRender(false);
00099
00100 threaded_nh_.setCallbackQueue(&threaded_queue_);
00101
00102 scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
00103
00104 target_scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00105
00106 Ogre::Light* directional_light = scene_manager_->createLight( "MainDirectional" );
00107 directional_light->setType( Ogre::Light::LT_DIRECTIONAL );
00108 directional_light->setDirection( Ogre::Vector3( -1, 0, -1 ) );
00109 directional_light->setDiffuseColour( Ogre::ColourValue( 1.0f, 1.0f, 1.0f ) );
00110
00111 root_display_group_ = new DisplayGroup();
00112 root_display_group_->setName( "root" );
00113 display_property_tree_model_ = new PropertyTreeModel( root_display_group_ );
00114 connect( display_property_tree_model_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00115
00116 tool_property_tree_model_ = new PropertyTreeModel( new Property() );
00117
00119
00120 global_options_ = new Property( "Global Options", QVariant(), "", root_display_group_ );
00121
00122 fixed_frame_property_ = new TfFrameProperty( "Fixed Frame", "/map",
00123 "Frame into which all data is transformed before being displayed.",
00124 global_options_, frame_manager_, false,
00125 SLOT( updateFixedFrame() ), this );
00126
00127 target_frame_property_ = new TfFrameProperty( "Target Frame", TfFrameProperty::FIXED_FRAME_STRING,
00128 "Reference frame for the 3D camera view.",
00129 global_options_, frame_manager_, true,
00130 SLOT( updateTargetFrame() ), this );
00131
00132 background_color_property_ = new ColorProperty( "Background Color", Qt::black,
00133 "Background color for the 3D view.",
00134 global_options_, SLOT( updateBackgroundColor() ), this );
00135
00136 root_display_group_->initialize( this );
00137 root_display_group_->setEnabled( true );
00138
00139 updateTargetFrame();
00140 updateFixedFrame();
00141 updateBackgroundColor();
00142
00143 global_status_ = new StatusList( "Global Status", root_display_group_ );
00144
00147
00148 createColorMaterials();
00149
00150 selection_manager_ = new SelectionManager(this);
00151
00152 threaded_queue_threads_.create_thread(boost::bind(&VisualizationManager::threadedQueueThreadFunc, this));
00153
00154 display_factory_ = new DisplayFactory();
00155
00156 tool_class_loader_ = new pluginlib::ClassLoader<Tool>( "rviz", "rviz::Tool" );
00157 }
00158
00159 VisualizationManager::~VisualizationManager()
00160 {
00161 delete update_timer_;
00162 delete idle_timer_;
00163
00164 shutting_down_ = true;
00165 threaded_queue_threads_.join_all();
00166
00167 if(selection_manager_)
00168 {
00169 selection_manager_->setSelection(M_Picked());
00170 }
00171
00172 delete display_property_tree_model_;
00173
00174 V_ToolRecord::iterator tool_it = tools_.begin();
00175 V_ToolRecord::iterator tool_end = tools_.end();
00176 for ( ; tool_it != tool_end; ++tool_it )
00177 {
00178 delete (*tool_it).tool;
00179 }
00180 tools_.clear();
00181
00182 delete display_factory_;
00183 delete tool_property_tree_model_;
00184 delete selection_manager_;
00185
00186 scene_manager_->destroySceneNode( target_scene_node_ );
00187
00188 if(ogre_root_)
00189 {
00190 ogre_root_->destroySceneManager( scene_manager_ );
00191 }
00192 delete frame_manager_;
00193 }
00194
00195 void VisualizationManager::initialize(const StatusCallback& cb, bool verbose)
00196 {
00197 if(cb)
00198 {
00199 cb("Initializing TF");
00200 }
00201
00202 render_panel_->getCamera()->setPosition(0, 10, 15);
00203 render_panel_->getCamera()->setNearClipDistance(0.01f);
00204 render_panel_->getCamera()->lookAt(0, 0, 0);
00205
00206 addViewController(XYOrbitViewController::getClassNameStatic(), "XYOrbit");
00207 addViewController(OrbitViewController::getClassNameStatic(), "Orbit");
00208 addViewController(FPSViewController::getClassNameStatic(), "FPS");
00209 addViewController(FixedOrientationOrthoViewController::getClassNameStatic(), "TopDownOrtho");
00210 setCurrentViewControllerType(OrbitViewController::getClassNameStatic());
00211
00212 selection_manager_->initialize( verbose );
00213
00214 last_update_ros_time_ = ros::Time::now();
00215 last_update_wall_time_ = ros::WallTime::now();
00216 }
00217
00218 void VisualizationManager::startUpdate()
00219 {
00220 update_timer_ = new QTimer;
00221 connect( update_timer_, SIGNAL( timeout() ), this, SLOT( onUpdate() ));
00222 update_timer_->start( 33 );
00223
00224 idle_timer_ = new QTimer;
00225 connect( idle_timer_, SIGNAL( timeout() ), this, SLOT( onIdle() ));
00226 idle_timer_->start( 33 );
00227 }
00228
00229 void createColorMaterial(const std::string& name, const Ogre::ColourValue& color)
00230 {
00231 Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( name, ROS_PACKAGE_NAME );
00232 mat->setAmbient(color * 0.5f);
00233 mat->setDiffuse(color);
00234 mat->setSelfIllumination(color);
00235 mat->setLightingEnabled(true);
00236 mat->setReceiveShadows(false);
00237 }
00238
00239 void VisualizationManager::createColorMaterials()
00240 {
00241 createColorMaterial("RVIZ/Red", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f));
00242 createColorMaterial("RVIZ/Green", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f));
00243 createColorMaterial("RVIZ/Blue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f));
00244 createColorMaterial("RVIZ/Cyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f));
00245 }
00246
00247 void VisualizationManager::queueRender()
00248 {
00249 render_requested_ = 1;
00250 }
00251
00252 void VisualizationManager::onUpdate()
00253 {
00254 if(disable_update_)
00255 {
00256 return;
00257 }
00258
00259 disable_update_ = true;
00260
00261
00262
00263 std::deque<ViewportMouseEvent> event_queue;
00264 {
00265 boost::mutex::scoped_lock lock(vme_queue_mutex_);
00266 event_queue.swap( vme_queue_ );
00267 }
00268
00269 std::deque<ViewportMouseEvent>::iterator event_it;
00270
00271 for ( event_it= event_queue.begin(); event_it!=event_queue.end(); event_it++ )
00272 {
00273 ViewportMouseEvent &vme = *event_it;
00274 int flags = 0;
00275 if( current_tool_ )
00276 {
00277 flags = current_tool_->processMouseEvent(vme);
00278 }
00279
00280 if( flags & Tool::Render )
00281 {
00282 queueRender();
00283 }
00284
00285 if( flags & Tool::Finished )
00286 {
00287 setCurrentTool( getDefaultTool() );
00288 }
00289 }
00290
00291
00292 ros::WallTime update_start = ros::WallTime::now();
00293
00294 ros::WallDuration wall_diff = ros::WallTime::now() - last_update_wall_time_;
00295 ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_;
00296 float wall_dt = wall_diff.toSec();
00297 float ros_dt = ros_diff.toSec();
00298
00299 if(ros_dt < 0.0)
00300 {
00301 resetTime();
00302 }
00303
00304 frame_manager_->update();
00305
00306 ros::spinOnce();
00307
00308 last_update_ros_time_ = ros::Time::now();
00309 last_update_wall_time_ = ros::WallTime::now();
00310
00311 root_display_group_->update( wall_dt, ros_dt );
00312
00313 view_controller_->update(wall_dt, ros_dt);
00314
00315 time_update_timer_ += wall_dt;
00316
00317 if( time_update_timer_ > 0.1f )
00318 {
00319 time_update_timer_ = 0.0f;
00320
00321 updateTime();
00322 }
00323
00324 frame_update_timer_ += wall_dt;
00325
00326 if(frame_update_timer_ > 1.0f)
00327 {
00328 frame_update_timer_ = 0.0f;
00329
00330 updateFrames();
00331 }
00332
00333 selection_manager_->update();
00334
00335 if( current_tool_ )
00336 {
00337 current_tool_->update(wall_dt, ros_dt);
00338 }
00339
00340 disable_update_ = false;
00341 }
00342
00343 void VisualizationManager::onIdle()
00344 {
00345 ros::WallTime cur = ros::WallTime::now();
00346 double dt = (cur - last_render_).toSec();
00347
00348 if(dt > 0.1f)
00349 {
00350 render_requested_ = 1;
00351 }
00352
00353
00354 if(render_requested_ && dt > 0.016f)
00355 {
00356 render_requested_ = 0;
00357 last_render_ = cur;
00358 frame_count_++;
00359
00360 boost::mutex::scoped_lock lock(render_mutex_);
00361
00362
00363 ogre_root_->renderOneFrame();
00364
00365
00366
00367 }
00368 }
00369
00370 void VisualizationManager::updateTime()
00371 {
00372 if( ros_time_begin_.isZero() )
00373 {
00374 ros_time_begin_ = ros::Time::now();
00375 }
00376
00377 ros_time_elapsed_ = ros::Time::now() - ros_time_begin_;
00378
00379 if( wall_clock_begin_.isZero() )
00380 {
00381 wall_clock_begin_ = ros::WallTime::now();
00382 }
00383
00384 wall_clock_elapsed_ = ros::WallTime::now() - wall_clock_begin_;
00385
00386 Q_EMIT timeChanged();
00387 }
00388
00389 void VisualizationManager::updateFrames()
00390 {
00391 typedef std::vector<std::string> V_string;
00392 V_string frames;
00393 frame_manager_->getTFClient()->getFrameStrings( frames );
00394
00395
00396 std::string error;
00397 if( frame_manager_->frameHasProblems( getFixedFrame().toStdString(), ros::Time(), error ))
00398 {
00399 if( frames.empty() )
00400 {
00401
00402 std::stringstream ss;
00403 ss << "No tf data. Actual error: " << error;
00404 global_status_->setStatus( StatusProperty::Warn, "Fixed Frame", QString::fromStdString( ss.str() ));
00405 }
00406 else
00407 {
00408
00409 global_status_->setStatus( StatusProperty::Error, "Fixed Frame", QString::fromStdString( error ));
00410 }
00411 }
00412 else
00413 {
00414
00415 global_status_->setStatus( StatusProperty::Ok, "Fixed Frame", "OK" );
00416 }
00417
00418 if( frame_manager_->transformHasProblems( getTargetFrame().toStdString(), ros::Time(), error ))
00419 {
00420
00421 global_status_->setStatus( StatusProperty::Error, "Target Frame", QString::fromStdString( error ));
00422 }
00423 else
00424 {
00425
00426 global_status_->setStatus( StatusProperty::Ok, "Target Frame", "OK" );
00427 }
00428 }
00429
00430 tf::TransformListener* VisualizationManager::getTFClient() const
00431 {
00432 return frame_manager_->getTFClient();
00433 }
00434
00435 void VisualizationManager::resetTime()
00436 {
00437 root_display_group_->reset();
00438 frame_manager_->getTFClient()->clear();
00439
00440 ros_time_begin_ = ros::Time();
00441 wall_clock_begin_ = ros::WallTime();
00442
00443 queueRender();
00444 }
00445
00446 void VisualizationManager::addDisplay( Display* display, bool enabled )
00447 {
00448 root_display_group_->addDisplay( display );
00449 display->initialize( this );
00450 display->setEnabled( enabled );
00451 }
00452
00453 void VisualizationManager::removeAllDisplays()
00454 {
00455 root_display_group_->removeAllDisplays();
00456 }
00457
00458 void VisualizationManager::setCurrentTool( Tool* tool )
00459 {
00460 if( current_tool_ )
00461 {
00462 current_tool_->deactivate();
00463 }
00464
00465 current_tool_ = tool;
00466 if( current_tool_ )
00467 {
00468 current_tool_->activate();
00469 }
00470
00471 Q_EMIT toolChanged( tool );
00472 }
00473
00474 void VisualizationManager::setDefaultTool( Tool* tool )
00475 {
00476 default_tool_ = tool;
00477 }
00478
00479 Tool* VisualizationManager::getTool( int index )
00480 {
00481 ROS_ASSERT( index >= 0 );
00482 ROS_ASSERT( index < (int)tools_.size() );
00483
00484 return tools_[ index ].tool;
00485 }
00486
00487 void VisualizationManager::removeTool( int index )
00488 {
00489 ToolRecord record = tools_[ index ];
00490 tools_.erase( tools_.begin() + index );
00491 if( current_tool_ == record.tool )
00492 {
00493 current_tool_ = NULL;
00494 }
00495 if( default_tool_ == record.tool )
00496 {
00497 default_tool_ = NULL;
00498 }
00499 Q_EMIT toolRemoved( record.tool );
00500 delete record.tool;
00501 }
00502
00503 std::set<std::string> VisualizationManager::getToolClasses()
00504 {
00505 std::set<std::string> class_names;
00506 V_ToolRecord::iterator tool_it;
00507 for ( tool_it = tools_.begin(); tool_it != tools_.end(); ++tool_it )
00508 {
00509 class_names.insert( (*tool_it).lookup_name );
00510 }
00511 return class_names;
00512 }
00513
00514 void VisualizationManager::load( const YAML::Node& yaml_node, const StatusCallback& cb )
00515 {
00516 disable_update_ = true;
00517
00518 if(cb)
00519 {
00520 cb("Creating displays");
00521 }
00522
00523 if( yaml_node.Type() != YAML::NodeType::Map )
00524 {
00525 printf( "VisualizationManager::load() TODO: error handling - unexpected YAML type.\n" );
00526 return;
00527 }
00528
00529 root_display_group_->load( yaml_node );
00530
00531 if(cb)
00532 {
00533 cb("Creating tools");
00534 }
00545 addTool( "rviz/MoveCamera" );
00546 addTool( "rviz/Interact" );
00547 addTool( "rviz/Select" );
00548
00550
00564
00565 disable_update_ = false;
00566 }
00567
00568 void VisualizationManager::addTool( const std::string& tool_class_lookup_name )
00569 {
00570 try
00571 {
00572 ToolRecord record;
00573 record.tool = tool_class_loader_->createUnmanagedInstance( tool_class_lookup_name );
00574 record.lookup_name = tool_class_lookup_name;
00575 tools_.push_back( record );
00576 record.tool->initialize( this );
00577
00578 Q_EMIT toolAdded( record.tool );
00579
00580
00581
00582 if( tools_.size() == 1 )
00583 {
00584 setDefaultTool( record.tool );
00585 setCurrentTool( record.tool );
00586 }
00587 }
00588 catch( pluginlib::PluginlibException& ex )
00589 {
00590 ROS_ERROR( "The plugin for class '%s' failed to load. Error: %s",
00591 tool_class_lookup_name.c_str(), ex.what() );
00592 }
00593 }
00594
00595 void VisualizationManager::save( YAML::Emitter& emitter )
00596 {
00597 root_display_group_->save( emitter );
00598
00617 }
00618
00619 Display* VisualizationManager::createDisplay( const QString& class_lookup_name,
00620 const QString& name,
00621 bool enabled )
00622 {
00623 Display* new_display = root_display_group_->createDisplay( class_lookup_name );
00624 new_display->setName( name );
00625 addDisplay( new_display, enabled );
00626 return new_display;
00627 }
00628
00629 double VisualizationManager::getWallClock()
00630 {
00631 return ros::WallTime::now().toSec();
00632 }
00633
00634 double VisualizationManager::getROSTime()
00635 {
00636 return (ros_time_begin_ + ros_time_elapsed_).toSec();
00637 }
00638
00639 double VisualizationManager::getWallClockElapsed()
00640 {
00641 return wall_clock_elapsed_.toSec();
00642 }
00643
00644 double VisualizationManager::getROSTimeElapsed()
00645 {
00646 return ros_time_elapsed_.toSec();
00647 }
00648
00649 void VisualizationManager::updateBackgroundColor()
00650 {
00651 render_panel_->setBackgroundColor( qtToOgre( background_color_property_->getColor() ));
00652
00653 queueRender();
00654 }
00655
00656 void VisualizationManager::handleChar( QKeyEvent* event, RenderPanel* panel )
00657 {
00658 if( event->key() == Qt::Key_Escape )
00659 {
00660 setCurrentTool( getDefaultTool() );
00661
00662 return;
00663 }
00664 if( current_tool_ )
00665 {
00666 current_tool_->processKeyEvent( event, panel );
00667 }
00668 }
00669
00670 void VisualizationManager::addViewController(const std::string& class_name, const std::string& name)
00671 {
00672 Q_EMIT viewControllerTypeAdded( class_name, name );
00673 }
00674
00675 bool VisualizationManager::setCurrentViewControllerType(const std::string& type)
00676 {
00677 if(view_controller_ && (view_controller_->getClassName() == type || view_controller_->getName() == type))
00678 {
00679 return true;
00680 }
00681
00682 bool found = true;
00683
00684 if(type == "rviz::OrbitViewController" || type == "Orbit")
00685 {
00686 view_controller_ = new OrbitViewController(this, "Orbit",target_scene_node_);
00687 }
00688 else if(type == "rviz::XYOrbitViewController" || type == "XYOrbit" ||
00689 type == "rviz::SimpleOrbitViewController" || type == "SimpleOrbit" )
00690 {
00691 view_controller_ = new XYOrbitViewController(this, "XYOrbit",target_scene_node_);
00692 }
00693 else if(type == "rviz::FPSViewController" || type == "FPS")
00694 {
00695 view_controller_ = new FPSViewController(this, "FPS",target_scene_node_);
00696 }
00697 else if(type == "rviz::FixedOrientationOrthoViewController" || type == "TopDownOrtho" || type == "Top-down Orthographic")
00698 {
00699 view_controller_ = new FixedOrientationOrthoViewController(this, "TopDownOrtho",target_scene_node_);
00700 }
00701 else if(!view_controller_)
00702 {
00703 view_controller_ = new OrbitViewController(this, "Orbit",target_scene_node_);
00704 }
00705 else
00706 {
00707 found = false;
00708 }
00709
00710 if(found)
00711 {
00712
00713
00714 render_panel_->setViewController(view_controller_);
00715 view_controller_->setTargetFrame( target_frame_property_->getValue().toString().toStdString() );
00716 connect( view_controller_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00717 Q_EMIT viewControllerChanged( view_controller_ );
00718 Q_EMIT configChanged();
00719 }
00720
00721 return found;
00722 }
00723
00724 std::string VisualizationManager::getCurrentViewControllerType()
00725 {
00726 return view_controller_->getClassName();
00727 }
00728
00729 void VisualizationManager::handleMouseEvent( const ViewportMouseEvent& vme )
00730 {
00731 boost::mutex::scoped_lock lock( vme_queue_mutex_ );
00732 vme_queue_.push_back(vme);
00733 }
00734
00735 void VisualizationManager::threadedQueueThreadFunc()
00736 {
00737 while (!shutting_down_)
00738 {
00739 threaded_queue_.callOne(ros::WallDuration(0.1));
00740 }
00741 }
00742
00743 void VisualizationManager::notifyConfigChanged()
00744 {
00745 Q_EMIT configChanged();
00746 }
00747
00748 void VisualizationManager::updateFixedFrame()
00749 {
00750 QString frame = fixed_frame_property_->getFrame();
00751
00752 frame_manager_->setFixedFrame( frame.toStdString() );
00753 root_display_group_->setFixedFrame( frame );
00754 }
00755
00756 void VisualizationManager::updateTargetFrame()
00757 {
00758 if( view_controller_ )
00759 {
00760 view_controller_->setTargetFrame( getTargetFrame().toStdString() );
00761 }
00762 }
00763
00764 QString VisualizationManager::getTargetFrame() const
00765 {
00766 return target_frame_property_->getFrame();
00767 }
00768
00769 QString VisualizationManager::getFixedFrame() const
00770 {
00771 return fixed_frame_property_->getFrame();
00772 }
00773
00774 void VisualizationManager::setFixedFrame( const QString& frame )
00775 {
00776 fixed_frame_property_->setValue( frame );
00777 }
00778
00779 void VisualizationManager::setTargetFrame( const QString& frame )
00780 {
00781 target_frame_property_->setValue( frame );
00782 }
00783
00784 }