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