visualization_manager.cpp

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <algorithm>

#include <QApplication>
#include <QCursor>
#include <QPixmap>
#include <QTimer>
#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
#include <QWindow>
#endif

#include <boost/bind.hpp>

#include <OgreRoot.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreLight.h>
#include <OgreViewport.h>
#include <OgreMaterialManager.h>
#include <OgreMaterial.h>
#include <OgreRenderWindow.h>
#include <OgreSharedPtr.h>
#include <OgreCamera.h>

#include <boost/filesystem.hpp>

#include <tf/transform_listener.h>

#include <ros/package.h>
#include <ros/callback_queue.h>

#include "rviz/display.h"
#include "rviz/display_factory.h"
#include "rviz/display_group.h"
#include "rviz/displays_panel.h"
#include "rviz/frame_manager.h"
#include "rviz/ogre_helpers/qt_ogre_render_window.h"
#include "rviz/properties/color_property.h"
#include "rviz/properties/parse_color.h"
#include "rviz/properties/property.h"
#include "rviz/properties/property_tree_model.h"
#include "rviz/properties/status_list.h"
#include "rviz/properties/tf_frame_property.h"
#include "rviz/properties/int_property.h"
#include "rviz/render_panel.h"
#include "rviz/selection/selection_manager.h"
#include "rviz/tool.h"
#include "rviz/tool_manager.h"
#include "rviz/viewport_mouse_event.h"
#include "rviz/view_controller.h"
#include "rviz/view_manager.h"
#include "rviz/load_resource.h"
#include "rviz/ogre_helpers/ogre_render_queue_clearer.h"
#include "rviz/ogre_helpers/render_system.h"

#include "rviz/visualization_manager.h"
#include "rviz/window_manager_interface.h"

namespace rviz
{

//helper class needed to display an icon besides "Global Options"
class IconizedProperty: public Property {
public:
  IconizedProperty( const QString& name = QString(),
              const QVariant default_value = QVariant(),
              const QString& description = QString(),
              Property* parent = 0,
              const char *changed_slot = 0,
              QObject* receiver = 0 )
  :Property( name, default_value, description, parent, changed_slot, receiver ) {};
  virtual QVariant getViewData( int column, int role ) const
  {
    return (column == 0 && role == Qt::DecorationRole)
        ? icon_ : Property::getViewData(column,role);
  }
  void setIcon( QIcon icon ) { icon_=icon; }
private:
  QIcon icon_;
};

class VisualizationManagerPrivate
{
public:
  ros::CallbackQueue threaded_queue_;
  boost::thread_group threaded_queue_threads_;
  ros::NodeHandle update_nh_;
  ros::NodeHandle threaded_nh_;
  boost::mutex render_mutex_;
};

VisualizationManager::VisualizationManager( RenderPanel* render_panel, WindowManagerInterface* wm, int tf_buffer_size, boost::shared_ptr<tf::TransformListener> tf )
: ogre_root_( Ogre::Root::getSingletonPtr() )
, update_timer_(0)
, shutting_down_(false)
, render_panel_( render_panel )
, time_update_timer_(0.0f)
, frame_update_timer_(0.0f)
, render_requested_(1)
, frame_count_(0)
, window_manager_(wm)
, private_( new VisualizationManagerPrivate )
{
  // visibility_bit_allocator_ is listed after default_visibility_bit_ (and thus initialized later be default):
  default_visibility_bit_ = visibility_bit_allocator_.allocBit();

  frame_manager_ = new FrameManager(tf, tf_buffer_size);

  render_panel->setAutoRender(false);

  private_->threaded_nh_.setCallbackQueue(&private_->threaded_queue_);

  scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );

  rviz::RenderSystem::RenderSystem::get()->prepareOverlays(scene_manager_);

  directional_light_ = scene_manager_->createLight( "MainDirectional" );
  directional_light_->setType( Ogre::Light::LT_DIRECTIONAL );
  directional_light_->setDirection( Ogre::Vector3( -1, 0, -1 ) );
  directional_light_->setDiffuseColour( Ogre::ColourValue( 1.0f, 1.0f, 1.0f ) );

  root_display_group_ = new DisplayGroup();
  root_display_group_->setName( "root" );
  display_property_tree_model_ = new PropertyTreeModel( root_display_group_ );
  display_property_tree_model_->setDragDropClass( "display" );
  connect( display_property_tree_model_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));

  tool_manager_ = new ToolManager( this );
  connect( tool_manager_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
  connect( tool_manager_, SIGNAL( toolChanged( Tool* ) ), this, SLOT( onToolChanged( Tool* ) ));

  view_manager_ = new ViewManager( this );
  view_manager_->setRenderPanel( render_panel_ );
  connect( view_manager_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));

  IconizedProperty* ip = new IconizedProperty( "Global Options", QVariant(), "", root_display_group_ );
  ip->setIcon( loadPixmap("package://rviz/icons/options.png") );
  global_options_ = ip;

  fixed_frame_property_ = new TfFrameProperty( "Fixed Frame", "/map",
                                               "Frame into which all data is transformed before being displayed.",
                                               global_options_, frame_manager_, false,
                                               SLOT( updateFixedFrame() ), this );

  background_color_property_ = new ColorProperty( "Background Color", QColor(48,48,48),
                                                  "Background color for the 3D view.",
                                                  global_options_, SLOT( updateBackgroundColor() ), this );

  fps_property_ = new IntProperty( "Frame Rate", 30,
                                   "RViz will try to render this many frames per second.",
                                   global_options_, SLOT( updateFps() ), this );

  root_display_group_->initialize( this ); // only initialize() a Display after its sub-properties are created.
  root_display_group_->setEnabled( true );

  updateFixedFrame();
  updateBackgroundColor();

  global_status_ = new StatusList( "Global Status", root_display_group_ );

  createColorMaterials();

  selection_manager_ = new SelectionManager(this);

  private_->threaded_queue_threads_.create_thread(boost::bind(&VisualizationManager::threadedQueueThreadFunc, this));

  display_factory_ = new DisplayFactory();

  ogre_render_queue_clearer_ = new OgreRenderQueueClearer();
  Ogre::Root::getSingletonPtr()->addFrameListener( ogre_render_queue_clearer_ );

  update_timer_ = new QTimer;
  connect( update_timer_, SIGNAL( timeout() ), this, SLOT( onUpdate() ));
}

VisualizationManager::~VisualizationManager()
{
  delete update_timer_;

  shutting_down_ = true;
  private_->threaded_queue_threads_.join_all();

  if(selection_manager_)
  {
    selection_manager_->setSelection(M_Picked());
  }

  delete display_property_tree_model_;
  delete tool_manager_;
  delete display_factory_;
  delete selection_manager_;

  if(ogre_root_)
  {
    ogre_root_->destroySceneManager( scene_manager_ );
  }
  delete frame_manager_;
  delete private_;

  Ogre::Root::getSingletonPtr()->removeFrameListener( ogre_render_queue_clearer_ );
  delete ogre_render_queue_clearer_;
}

void VisualizationManager::initialize()
{
  emitStatusUpdate( "Initializing managers." );

  view_manager_->initialize();
  selection_manager_->initialize();
  tool_manager_->initialize();

  last_update_ros_time_ = ros::Time::now();
  last_update_wall_time_ = ros::WallTime::now();
}

ros::CallbackQueueInterface* VisualizationManager::getThreadedQueue()
{
  return &private_->threaded_queue_;
}

void VisualizationManager::lockRender()
{
  private_->render_mutex_.lock();
}

void VisualizationManager::unlockRender()
{
  private_->render_mutex_.unlock();
}

ros::CallbackQueueInterface* VisualizationManager::getUpdateQueue()
{
  return ros::getGlobalCallbackQueue();
}

void VisualizationManager::startUpdate()
{
  float interval = 1000.0 / float(fps_property_->getInt());
  update_timer_->start( interval );
}

void VisualizationManager::stopUpdate()
{
  update_timer_->stop();
}

void createColorMaterial(const std::string& name, const Ogre::ColourValue& color, bool use_self_illumination)
{
  Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( name, ROS_PACKAGE_NAME );
  mat->setAmbient(color * 0.5f);
  mat->setDiffuse(color);
  if( use_self_illumination )
  {
    mat->setSelfIllumination(color);
  }
  mat->setLightingEnabled(true);
  mat->setReceiveShadows(false);
}

void VisualizationManager::createColorMaterials()
{
  createColorMaterial("RVIZ/Red", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f), true);
  createColorMaterial("RVIZ/Green", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f), true);
  createColorMaterial("RVIZ/Blue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f), true);
  createColorMaterial("RVIZ/Cyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f), true);
  createColorMaterial("RVIZ/ShadedRed", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f), false);
  createColorMaterial("RVIZ/ShadedGreen", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f), false);
  createColorMaterial("RVIZ/ShadedBlue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f), false);
  createColorMaterial("RVIZ/ShadedCyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f), false);
}

void VisualizationManager::queueRender()
{
  render_requested_ = 1;
}

void VisualizationManager::onUpdate()
{
  ros::WallDuration wall_diff = ros::WallTime::now() - last_update_wall_time_;
  ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_;
  float wall_dt = wall_diff.toSec();
  float ros_dt = ros_diff.toSec();
  last_update_ros_time_ = ros::Time::now();
  last_update_wall_time_ = ros::WallTime::now();

  if(ros_dt < 0.0)
  {
    resetTime();
  }

  ros::spinOnce();

  Q_EMIT preUpdate();

  frame_manager_->update();

  root_display_group_->update( wall_dt, ros_dt );

  view_manager_->update(wall_dt, ros_dt);

  time_update_timer_ += wall_dt;

  if( time_update_timer_ > 0.1f )
  {
    time_update_timer_ = 0.0f;

    updateTime();
  }

  frame_update_timer_ += wall_dt;

  if(frame_update_timer_ > 1.0f)
  {
    frame_update_timer_ = 0.0f;

    updateFrames();
  }

  selection_manager_->update();

  if( tool_manager_->getCurrentTool() )
  {
    tool_manager_->getCurrentTool()->update(wall_dt, ros_dt);
  }

  if ( view_manager_ &&
        view_manager_->getCurrent() &&
        view_manager_->getCurrent()->getCamera() )
  {
    directional_light_->setDirection(view_manager_->getCurrent()->getCamera()->getDerivedDirection());
  }

  frame_count_++;

  if ( render_requested_ || wall_dt > 0.01 )
  {
    render_requested_ = 0;
    boost::mutex::scoped_lock lock(private_->render_mutex_);
    ogre_root_->renderOneFrame();
  }
}

void VisualizationManager::updateTime()
{
  if( ros_time_begin_.isZero() )
  {
    ros_time_begin_ = ros::Time::now();
  }

  ros_time_elapsed_ = ros::Time::now() - ros_time_begin_;

  if( wall_clock_begin_.isZero() )
  {
    wall_clock_begin_ = ros::WallTime::now();
  }

  wall_clock_elapsed_ = ros::WallTime::now() - wall_clock_begin_;
}

void VisualizationManager::updateFrames()
{
  typedef std::vector<std::string> V_string;
  V_string frames;
  frame_manager_->getTFClient()->getFrameStrings( frames );

  // Check the fixed frame to see if it's ok
  std::string error;
  if( frame_manager_->frameHasProblems( getFixedFrame().toStdString(), ros::Time(), error ))
  {
    if( frames.empty() )
    {
      // fixed_prop->setToWarn();
      std::stringstream ss;
      ss << "No tf data.  Actual error: " << error;
      global_status_->setStatus( StatusProperty::Warn, "Fixed Frame", QString::fromStdString( ss.str() ));
    }
    else
    {
      // fixed_prop->setToError();
      global_status_->setStatus( StatusProperty::Error, "Fixed Frame", QString::fromStdString( error ));
    }
  }
  else
  {
    // fixed_prop->setToOK();
    global_status_->setStatus( StatusProperty::Ok, "Fixed Frame", "OK" );
  }
}

tf::TransformListener* VisualizationManager::getTFClient() const
{
  return frame_manager_->getTFClient();
}

void VisualizationManager::resetTime()
{
  root_display_group_->reset();
  frame_manager_->getTFClient()->clear();

  ros_time_begin_ = ros::Time();
  wall_clock_begin_ = ros::WallTime();

  queueRender();
}

void VisualizationManager::addDisplay( Display* display, bool enabled )
{
  root_display_group_->addDisplay( display );
  display->initialize( this );
  display->setEnabled( enabled );
}

void VisualizationManager::removeAllDisplays()
{
  root_display_group_->removeAllDisplays();
}

void VisualizationManager::emitStatusUpdate( const QString& message )
{
  Q_EMIT statusUpdate( message );
}

void VisualizationManager::load( const Config& config )
{
  stopUpdate();

  emitStatusUpdate( "Creating displays" );
  root_display_group_->load( config );

  emitStatusUpdate( "Creating tools" );
  tool_manager_->load( config.mapGetChild( "Tools" ));

  emitStatusUpdate( "Creating views" );
  view_manager_->load( config.mapGetChild( "Views" ));

  startUpdate();
}

void VisualizationManager::save( Config config ) const
{
  root_display_group_->save( config );
  tool_manager_->save( config.mapMakeChild( "Tools" ));
  view_manager_->save( config.mapMakeChild( "Views" ));
}

Display* VisualizationManager::createDisplay( const QString& class_lookup_name,
                                              const QString& name,
                                              bool enabled )
{
  QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
  Display* new_display = root_display_group_->createDisplay( class_lookup_name );
  addDisplay( new_display, enabled );
  new_display->setName( name );
  QApplication::restoreOverrideCursor();
  return new_display;
}

double VisualizationManager::getWallClock()
{
  return ros::WallTime::now().toSec();
}

double VisualizationManager::getROSTime()
{
  return frame_manager_->getTime().toSec();
}

double VisualizationManager::getWallClockElapsed()
{
  return wall_clock_elapsed_.toSec();
}

double VisualizationManager::getROSTimeElapsed()
{
  return (frame_manager_->getTime() - ros_time_begin_).toSec();
}

void VisualizationManager::updateBackgroundColor()
{
  render_panel_->setBackgroundColor( qtToOgre( background_color_property_->getColor() ));

  queueRender();
}

void VisualizationManager::updateFps()
{
  if ( update_timer_->isActive() )
  {
    startUpdate();
  }
}

void VisualizationManager::handleMouseEvent( const ViewportMouseEvent& vme )
{
  //process pending mouse events
  Tool* current_tool = tool_manager_->getCurrentTool();

  int flags = 0;
  if( current_tool )
  {
    ViewportMouseEvent _vme = vme;
#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
    QWindow* window = vme.panel->windowHandle();
    if (window)
    {
        double pixel_ratio = window->devicePixelRatio();
        _vme.x = static_cast<int>(pixel_ratio * _vme.x);
        _vme.y = static_cast<int>(pixel_ratio * _vme.y);
        _vme.last_x = static_cast<int>(pixel_ratio * _vme.last_x);
        _vme.last_y = static_cast<int>(pixel_ratio * _vme.last_y);
    }
#endif
    flags = current_tool->processMouseEvent( _vme );
    vme.panel->setCursor( current_tool->getCursor() );
  }
  else
  {
    vme.panel->setCursor( QCursor( Qt::ArrowCursor ) );
  }

  if( flags & Tool::Render )
  {
    queueRender();
  }

  if( flags & Tool::Finished )
  {
    tool_manager_->setCurrentTool( tool_manager_->getDefaultTool() );
  }
}

void VisualizationManager::handleChar( QKeyEvent* event, RenderPanel* panel )
{
  tool_manager_->handleChar( event, panel );
}

void VisualizationManager::threadedQueueThreadFunc()
{
  while (!shutting_down_)
  {
    private_->threaded_queue_.callOne(ros::WallDuration(0.1));
  }
}

void VisualizationManager::notifyConfigChanged()
{
  Q_EMIT configChanged();
}

void VisualizationManager::onToolChanged( Tool* tool )
{
}

void VisualizationManager::updateFixedFrame()
{
  QString frame = fixed_frame_property_->getFrame();

  frame_manager_->setFixedFrame( frame.toStdString() );
  root_display_group_->setFixedFrame( frame );
}

QString VisualizationManager::getFixedFrame() const
{
  return fixed_frame_property_->getFrame();
}

void VisualizationManager::setFixedFrame( const QString& frame )
{
  fixed_frame_property_->setValue( frame );
}

void VisualizationManager::setStatus( const QString & message )
{
  emitStatusUpdate( message );
}

} // namespace rviz