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 #include <QCursor>
00034 #include <QPixmap>
00035 #include <QTimer>
00036
00037 #include <boost/bind.hpp>
00038
00039 #include <OgreRoot.h>
00040 #include <OgreSceneManager.h>
00041 #include <OgreSceneNode.h>
00042 #include <OgreLight.h>
00043 #include <OgreViewport.h>
00044 #include <OgreMaterialManager.h>
00045 #include <OgreMaterial.h>
00046 #include <OgreRenderWindow.h>
00047 #include <OgreSharedPtr.h>
00048 #include <OgreCamera.h>
00049
00050 #include <boost/filesystem.hpp>
00051
00052 #include <tf/transform_listener.h>
00053
00054 #include <ros/package.h>
00055 #include <ros/callback_queue.h>
00056
00057 #include "rviz/display.h"
00058 #include "rviz/display_factory.h"
00059 #include "rviz/display_group.h"
00060 #include "rviz/displays_panel.h"
00061 #include "rviz/frame_manager.h"
00062 #include "rviz/ogre_helpers/qt_ogre_render_window.h"
00063 #include "rviz/properties/color_property.h"
00064 #include "rviz/properties/parse_color.h"
00065 #include "rviz/properties/property.h"
00066 #include "rviz/properties/property_tree_model.h"
00067 #include "rviz/properties/status_list.h"
00068 #include "rviz/properties/tf_frame_property.h"
00069 #include "rviz/properties/int_property.h"
00070 #include "rviz/render_panel.h"
00071 #include "rviz/selection/selection_manager.h"
00072 #include "rviz/tool.h"
00073 #include "rviz/tool_manager.h"
00074 #include "rviz/viewport_mouse_event.h"
00075 #include "rviz/view_controller.h"
00076 #include "rviz/view_manager.h"
00077 #include "rviz/load_resource.h"
00078 #include "rviz/ogre_helpers/ogre_render_queue_clearer.h"
00079 #include "rviz/ogre_helpers/render_system.h"
00080
00081 #include "rviz/visualization_manager.h"
00082 #include "rviz/window_manager_interface.h"
00083
00084 namespace rviz
00085 {
00086
00087
00088 class IconizedProperty: public Property {
00089 public:
00090 IconizedProperty( const QString& name = QString(),
00091 const QVariant default_value = QVariant(),
00092 const QString& description = QString(),
00093 Property* parent = 0,
00094 const char *changed_slot = 0,
00095 QObject* receiver = 0 )
00096 :Property( name, default_value, description, parent, changed_slot, receiver ) {};
00097 virtual QVariant getViewData( int column, int role ) const
00098 {
00099 return (column == 0 && role == Qt::DecorationRole)
00100 ? icon_ : Property::getViewData(column,role);
00101 }
00102 void setIcon( QIcon icon ) { icon_=icon; }
00103 private:
00104 QIcon icon_;
00105 };
00106
00107 class VisualizationManagerPrivate
00108 {
00109 public:
00110 ros::CallbackQueue threaded_queue_;
00111 boost::thread_group threaded_queue_threads_;
00112 ros::NodeHandle update_nh_;
00113 ros::NodeHandle threaded_nh_;
00114 boost::mutex render_mutex_;
00115 };
00116
00117 VisualizationManager::VisualizationManager( RenderPanel* render_panel, WindowManagerInterface* wm )
00118 : ogre_root_( Ogre::Root::getSingletonPtr() )
00119 , update_timer_(0)
00120 , shutting_down_(false)
00121 , render_panel_( render_panel )
00122 , time_update_timer_(0.0f)
00123 , frame_update_timer_(0.0f)
00124 , render_requested_(1)
00125 , frame_count_(0)
00126 , window_manager_(wm)
00127 , private_( new VisualizationManagerPrivate )
00128 , default_visibility_bit_( visibility_bit_allocator_.allocBit() )
00129 {
00130 frame_manager_ = new FrameManager();
00131
00132 render_panel->setAutoRender(false);
00133
00134 private_->threaded_nh_.setCallbackQueue(&private_->threaded_queue_);
00135
00136 scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
00137
00138 rviz::RenderSystem::RenderSystem::get()->prepareOverlays(scene_manager_);
00139
00140 directional_light_ = scene_manager_->createLight( "MainDirectional" );
00141 directional_light_->setType( Ogre::Light::LT_DIRECTIONAL );
00142 directional_light_->setDirection( Ogre::Vector3( -1, 0, -1 ) );
00143 directional_light_->setDiffuseColour( Ogre::ColourValue( 1.0f, 1.0f, 1.0f ) );
00144
00145 root_display_group_ = new DisplayGroup();
00146 root_display_group_->setName( "root" );
00147 display_property_tree_model_ = new PropertyTreeModel( root_display_group_ );
00148 display_property_tree_model_->setDragDropClass( "display" );
00149 connect( display_property_tree_model_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00150
00151 tool_manager_ = new ToolManager( this );
00152 connect( tool_manager_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00153 connect( tool_manager_, SIGNAL( toolChanged( Tool* ) ), this, SLOT( onToolChanged( Tool* ) ));
00154
00155 view_manager_ = new ViewManager( this );
00156 view_manager_->setRenderPanel( render_panel_ );
00157 connect( view_manager_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00158
00159 IconizedProperty* ip = new IconizedProperty( "Global Options", QVariant(), "", root_display_group_ );
00160 ip->setIcon( loadPixmap("package://rviz/icons/options.png") );
00161 global_options_ = ip;
00162
00163 fixed_frame_property_ = new TfFrameProperty( "Fixed Frame", "/map",
00164 "Frame into which all data is transformed before being displayed.",
00165 global_options_, frame_manager_, false,
00166 SLOT( updateFixedFrame() ), this );
00167
00168 background_color_property_ = new ColorProperty( "Background Color", QColor(48,48,48),
00169 "Background color for the 3D view.",
00170 global_options_, SLOT( updateBackgroundColor() ), this );
00171
00172 fps_property_ = new IntProperty( "Frame Rate", 30,
00173 "RViz will try to render this many frames per second.",
00174 global_options_, SLOT( updateFps() ), this );
00175
00176 root_display_group_->initialize( this );
00177 root_display_group_->setEnabled( true );
00178
00179 updateFixedFrame();
00180 updateBackgroundColor();
00181
00182 global_status_ = new StatusList( "Global Status", root_display_group_ );
00183
00184 createColorMaterials();
00185
00186 selection_manager_ = new SelectionManager(this);
00187
00188 private_->threaded_queue_threads_.create_thread(boost::bind(&VisualizationManager::threadedQueueThreadFunc, this));
00189
00190 display_factory_ = new DisplayFactory();
00191
00192 ogre_render_queue_clearer_ = new OgreRenderQueueClearer();
00193 Ogre::Root::getSingletonPtr()->addFrameListener( ogre_render_queue_clearer_ );
00194
00195 update_timer_ = new QTimer;
00196 connect( update_timer_, SIGNAL( timeout() ), this, SLOT( onUpdate() ));
00197 }
00198
00199 VisualizationManager::~VisualizationManager()
00200 {
00201 delete update_timer_;
00202
00203 shutting_down_ = true;
00204 private_->threaded_queue_threads_.join_all();
00205
00206 if(selection_manager_)
00207 {
00208 selection_manager_->setSelection(M_Picked());
00209 }
00210
00211 delete display_property_tree_model_;
00212 delete tool_manager_;
00213 delete display_factory_;
00214 delete selection_manager_;
00215
00216 if(ogre_root_)
00217 {
00218 ogre_root_->destroySceneManager( scene_manager_ );
00219 }
00220 delete frame_manager_;
00221 delete private_;
00222
00223 Ogre::Root::getSingletonPtr()->removeFrameListener( ogre_render_queue_clearer_ );
00224 delete ogre_render_queue_clearer_;
00225 }
00226
00227 void VisualizationManager::initialize()
00228 {
00229 emitStatusUpdate( "Initializing managers." );
00230
00231 view_manager_->initialize();
00232 selection_manager_->initialize();
00233 tool_manager_->initialize();
00234
00235 last_update_ros_time_ = ros::Time::now();
00236 last_update_wall_time_ = ros::WallTime::now();
00237 }
00238
00239 ros::CallbackQueueInterface* VisualizationManager::getThreadedQueue()
00240 {
00241 return &private_->threaded_queue_;
00242 }
00243
00244 void VisualizationManager::lockRender()
00245 {
00246 private_->render_mutex_.lock();
00247 }
00248
00249 void VisualizationManager::unlockRender()
00250 {
00251 private_->render_mutex_.unlock();
00252 }
00253
00254 ros::CallbackQueueInterface* VisualizationManager::getUpdateQueue()
00255 {
00256 return ros::getGlobalCallbackQueue();
00257 }
00258
00259 void VisualizationManager::startUpdate()
00260 {
00261 float interval = 1000.0 / float(fps_property_->getInt());
00262 update_timer_->start( interval );
00263 }
00264
00265 void VisualizationManager::stopUpdate()
00266 {
00267 update_timer_->stop();
00268 }
00269
00270 void createColorMaterial(const std::string& name, const Ogre::ColourValue& color, bool use_self_illumination)
00271 {
00272 Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( name, ROS_PACKAGE_NAME );
00273 mat->setAmbient(color * 0.5f);
00274 mat->setDiffuse(color);
00275 if( use_self_illumination )
00276 {
00277 mat->setSelfIllumination(color);
00278 }
00279 mat->setLightingEnabled(true);
00280 mat->setReceiveShadows(false);
00281 }
00282
00283 void VisualizationManager::createColorMaterials()
00284 {
00285 createColorMaterial("RVIZ/Red", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f), true);
00286 createColorMaterial("RVIZ/Green", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f), true);
00287 createColorMaterial("RVIZ/Blue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f), true);
00288 createColorMaterial("RVIZ/Cyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f), true);
00289 createColorMaterial("RVIZ/ShadedRed", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f), false);
00290 createColorMaterial("RVIZ/ShadedGreen", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f), false);
00291 createColorMaterial("RVIZ/ShadedBlue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f), false);
00292 createColorMaterial("RVIZ/ShadedCyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f), false);
00293 }
00294
00295 void VisualizationManager::queueRender()
00296 {
00297 render_requested_ = 1;
00298 }
00299
00300 void VisualizationManager::onUpdate()
00301 {
00302 ros::WallDuration wall_diff = ros::WallTime::now() - last_update_wall_time_;
00303 ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_;
00304 float wall_dt = wall_diff.toSec();
00305 float ros_dt = ros_diff.toSec();
00306 last_update_ros_time_ = ros::Time::now();
00307 last_update_wall_time_ = ros::WallTime::now();
00308
00309 if(ros_dt < 0.0)
00310 {
00311 resetTime();
00312 }
00313
00314 ros::spinOnce();
00315
00316 Q_EMIT preUpdate();
00317
00318 frame_manager_->update();
00319
00320 root_display_group_->update( wall_dt, ros_dt );
00321
00322 view_manager_->update(wall_dt, ros_dt);
00323
00324 time_update_timer_ += wall_dt;
00325
00326 if( time_update_timer_ > 0.1f )
00327 {
00328 time_update_timer_ = 0.0f;
00329
00330 updateTime();
00331 }
00332
00333 frame_update_timer_ += wall_dt;
00334
00335 if(frame_update_timer_ > 1.0f)
00336 {
00337 frame_update_timer_ = 0.0f;
00338
00339 updateFrames();
00340 }
00341
00342 selection_manager_->update();
00343
00344 if( tool_manager_->getCurrentTool() )
00345 {
00346 tool_manager_->getCurrentTool()->update(wall_dt, ros_dt);
00347 }
00348
00349 if ( view_manager_ &&
00350 view_manager_->getCurrent() &&
00351 view_manager_->getCurrent()->getCamera() )
00352 {
00353 directional_light_->setDirection(view_manager_->getCurrent()->getCamera()->getDerivedDirection());
00354 }
00355
00356 frame_count_++;
00357
00358 if ( render_requested_ || wall_dt > 0.01 )
00359 {
00360 render_requested_ = 0;
00361 boost::mutex::scoped_lock lock(private_->render_mutex_);
00362 ogre_root_->renderOneFrame();
00363 }
00364 }
00365
00366 void VisualizationManager::updateTime()
00367 {
00368 if( ros_time_begin_.isZero() )
00369 {
00370 ros_time_begin_ = ros::Time::now();
00371 }
00372
00373 ros_time_elapsed_ = ros::Time::now() - ros_time_begin_;
00374
00375 if( wall_clock_begin_.isZero() )
00376 {
00377 wall_clock_begin_ = ros::WallTime::now();
00378 }
00379
00380 wall_clock_elapsed_ = ros::WallTime::now() - wall_clock_begin_;
00381 }
00382
00383 void VisualizationManager::updateFrames()
00384 {
00385 typedef std::vector<std::string> V_string;
00386 V_string frames;
00387 frame_manager_->getTFClient()->getFrameStrings( frames );
00388
00389
00390 std::string error;
00391 if( frame_manager_->frameHasProblems( getFixedFrame().toStdString(), ros::Time(), error ))
00392 {
00393 if( frames.empty() )
00394 {
00395
00396 std::stringstream ss;
00397 ss << "No tf data. Actual error: " << error;
00398 global_status_->setStatus( StatusProperty::Warn, "Fixed Frame", QString::fromStdString( ss.str() ));
00399 }
00400 else
00401 {
00402
00403 global_status_->setStatus( StatusProperty::Error, "Fixed Frame", QString::fromStdString( error ));
00404 }
00405 }
00406 else
00407 {
00408
00409 global_status_->setStatus( StatusProperty::Ok, "Fixed Frame", "OK" );
00410 }
00411 }
00412
00413 tf::TransformListener* VisualizationManager::getTFClient() const
00414 {
00415 return frame_manager_->getTFClient();
00416 }
00417
00418 void VisualizationManager::resetTime()
00419 {
00420 root_display_group_->reset();
00421 frame_manager_->getTFClient()->clear();
00422
00423 ros_time_begin_ = ros::Time();
00424 wall_clock_begin_ = ros::WallTime();
00425
00426 queueRender();
00427 }
00428
00429 void VisualizationManager::addDisplay( Display* display, bool enabled )
00430 {
00431 root_display_group_->addDisplay( display );
00432 display->initialize( this );
00433 display->setEnabled( enabled );
00434 }
00435
00436 void VisualizationManager::removeAllDisplays()
00437 {
00438 root_display_group_->removeAllDisplays();
00439 }
00440
00441 void VisualizationManager::emitStatusUpdate( const QString& message )
00442 {
00443 Q_EMIT statusUpdate( message );
00444 }
00445
00446 void VisualizationManager::load( const Config& config )
00447 {
00448 stopUpdate();
00449
00450 emitStatusUpdate( "Creating displays" );
00451 root_display_group_->load( config );
00452
00453 emitStatusUpdate( "Creating tools" );
00454 tool_manager_->load( config.mapGetChild( "Tools" ));
00455
00456 emitStatusUpdate( "Creating views" );
00457 view_manager_->load( config.mapGetChild( "Views" ));
00458
00459 startUpdate();
00460 }
00461
00462 void VisualizationManager::save( Config config ) const
00463 {
00464 root_display_group_->save( config );
00465 tool_manager_->save( config.mapMakeChild( "Tools" ));
00466 view_manager_->save( config.mapMakeChild( "Views" ));
00467 }
00468
00469 Display* VisualizationManager::createDisplay( const QString& class_lookup_name,
00470 const QString& name,
00471 bool enabled )
00472 {
00473 QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
00474 Display* new_display = root_display_group_->createDisplay( class_lookup_name );
00475 addDisplay( new_display, enabled );
00476 new_display->setName( name );
00477 QApplication::restoreOverrideCursor();
00478 return new_display;
00479 }
00480
00481 double VisualizationManager::getWallClock()
00482 {
00483 return ros::WallTime::now().toSec();
00484 }
00485
00486 double VisualizationManager::getROSTime()
00487 {
00488 return frame_manager_->getTime().toSec();
00489 }
00490
00491 double VisualizationManager::getWallClockElapsed()
00492 {
00493 return wall_clock_elapsed_.toSec();
00494 }
00495
00496 double VisualizationManager::getROSTimeElapsed()
00497 {
00498 return (frame_manager_->getTime() - ros_time_begin_).toSec();
00499 }
00500
00501 void VisualizationManager::updateBackgroundColor()
00502 {
00503 render_panel_->setBackgroundColor( qtToOgre( background_color_property_->getColor() ));
00504
00505 queueRender();
00506 }
00507
00508 void VisualizationManager::updateFps()
00509 {
00510 if ( update_timer_->isActive() )
00511 {
00512 startUpdate();
00513 }
00514 }
00515
00516 void VisualizationManager::handleMouseEvent( const ViewportMouseEvent& vme )
00517 {
00518
00519 Tool* current_tool = tool_manager_->getCurrentTool();
00520
00521 int flags = 0;
00522 if( current_tool )
00523 {
00524 ViewportMouseEvent _vme = vme;
00525 flags = current_tool->processMouseEvent( _vme );
00526 vme.panel->setCursor( current_tool->getCursor() );
00527 }
00528 else
00529 {
00530 vme.panel->setCursor( QCursor( Qt::ArrowCursor ) );
00531 }
00532
00533 if( flags & Tool::Render )
00534 {
00535 queueRender();
00536 }
00537
00538 if( flags & Tool::Finished )
00539 {
00540 tool_manager_->setCurrentTool( tool_manager_->getDefaultTool() );
00541 }
00542 }
00543
00544 void VisualizationManager::handleChar( QKeyEvent* event, RenderPanel* panel )
00545 {
00546 tool_manager_->handleChar( event, panel );
00547 }
00548
00549 void VisualizationManager::threadedQueueThreadFunc()
00550 {
00551 while (!shutting_down_)
00552 {
00553 private_->threaded_queue_.callOne(ros::WallDuration(0.1));
00554 }
00555 }
00556
00557 void VisualizationManager::notifyConfigChanged()
00558 {
00559 Q_EMIT configChanged();
00560 }
00561
00562 void VisualizationManager::onToolChanged( Tool* tool )
00563 {
00564 }
00565
00566 void VisualizationManager::updateFixedFrame()
00567 {
00568 QString frame = fixed_frame_property_->getFrame();
00569
00570 frame_manager_->setFixedFrame( frame.toStdString() );
00571 root_display_group_->setFixedFrame( frame );
00572 }
00573
00574 QString VisualizationManager::getFixedFrame() const
00575 {
00576 return fixed_frame_property_->getFrame();
00577 }
00578
00579 void VisualizationManager::setFixedFrame( const QString& frame )
00580 {
00581 fixed_frame_property_->setValue( frame );
00582 }
00583
00584 void VisualizationManager::setStatus( const QString & message )
00585 {
00586 emitStatusUpdate( message );
00587 }
00588
00589 }