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