object_recognition_rviz_ui.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, 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 "object_recognition_gui/object_recognition_rviz_ui.h"

#include "rviz_interaction_tools/image_tools.h"

#include "rviz_interaction_tools/mesh_object_switcher.h"
#include "rviz_interaction_tools/mesh_object.h"
#include "rviz_interaction_tools/camera_tools.h"

#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreRenderWindow.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreRoot.h>

#include <rviz/render_panel.h>
#include <rviz/window_manager_interface.h>
#include "rviz/display_context.h"

#include "rviz_interaction_tools/image_overlay.h"
#include "rviz_interaction_tools/camera_tools.h"
#include "rviz_interaction_tools/unique_string_manager.h"

#include "ui_object_recognition_frame.h" // generated by uic during build.

using namespace rviz_interaction_tools;

namespace object_recognition_gui
{

// material names for the different mesh colors
static const std::string VALID_MAT_NAME = "object_recognition_valid";
static const std::string VALID_SEL_MAT_NAME = "object_recognition_valid_selected";
static const std::string INVALID_MAT_NAME = "object_recognition_invalid";
static const std::string INVALID_SEL_MAT_NAME = "object_recognition_invalid_selected";


ObjectRecognitionRvizUI::ObjectRecognitionRvizUI(rviz::DisplayContext* context) :
    QWidget(),
    object_recognition_server_(0),
    ui_( new Ui::ObjectRecognitionFrame )
{
  ui_->setupUi( this );
  render_panel_ = ui_->render_panel_;
  connect( render_panel_, SIGNAL( mouseEvent( QMouseEvent* )),
           this, SLOT( onRenderWindowMouseEvent( QMouseEvent* )));
  connect( ui_->cancel, SIGNAL( clicked() ), this, SLOT( cancelButtonClicked() ));
  connect( ui_->ok, SIGNAL( clicked() ), this, SLOT( acceptButtonClicked() ));

  UniqueStringManager usm;
  
  //construct basic ogre scene
  scene_manager_ = Ogre::Root::getSingleton().createSceneManager( Ogre::ST_GENERIC, usm.unique("ObjectRecognitionRvizUI") );

  scene_root_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  
  image_overlay_ = new rviz_interaction_tools::ImageOverlay( scene_root_, Ogre::RENDER_QUEUE_BACKGROUND );

  setupRenderPanel( context );

  createMaterials();

  ray_scene_query_ = scene_manager_->createRayQuery(Ogre::Ray());
}

ObjectRecognitionRvizUI::~ObjectRecognitionRvizUI()
{
  if ( object_recognition_server_ )
  {
    stopActionServer();
  }

  cleanupAndHide();

  scene_manager_->destroyQuery( ray_scene_query_ );

  render_panel_->getRenderWindow()->setActive(false);

  delete render_panel_;
  delete image_overlay_;
  delete ui_;

  Ogre::Root::getSingleton().destroySceneManager( scene_manager_ );
}


void ObjectRecognitionRvizUI::showBoundingBoxes( bool show )
{
  scene_manager_->showBoundingBoxes(show);
}

bool ObjectRecognitionRvizUI::getShowBoundingBoxes()
{
  return scene_manager_->getShowBoundingBoxes();
}

void ObjectRecognitionRvizUI::updateBoundingBoxes()
{
  bool show = getShowBoundingBoxes();
  showBoundingBoxes (show);
}


void ObjectRecognitionRvizUI::cleanupAndHide()
{
  for ( size_t m=0; m<mesh_switchers_.size(); ++m )
  {
    delete mesh_switchers_[m];
  }
  mesh_switchers_.clear();
  hide();
}


void ObjectRecognitionRvizUI::setupRenderPanel( rviz::DisplayContext* context )
{
  render_panel_->initialize( scene_manager_, context );

  render_panel_->setAutoRender(false);
  render_panel_->getViewport()->setOverlaysEnabled(false);
  render_panel_->getViewport()->setClearEveryFrame(true);
  render_panel_->getRenderWindow()->setAutoUpdated(false);
  render_panel_->getRenderWindow()->setActive(true);

  //fill camera matrix with dummy values to RaySceneQuery won't crash
  render_panel_->getCamera()->setPosition(0.0, 0.0, 0.0);
  render_panel_->getCamera()->lookAt(Ogre::Vector3(0, 0, 1));
  render_panel_->getCamera()->roll(Ogre::Radian(3.141592653));
  render_panel_->getCamera()->setFOVy(Ogre::Radian(0.518));
  render_panel_->getCamera()->setAspectRatio(640.0 / 480.0);
  render_panel_->getCamera()->setNearClipDistance( 0.01f );
}


void ObjectRecognitionRvizUI::createMaterials()
{
  Ogre::MaterialPtr white = Ogre::MaterialManager::getSingleton().getByName("BaseWhiteNoLighting");

  Ogre::Pass *pass;

  // clone materials + adjust rendering settings

  // material for valid models
  Ogre::MaterialPtr valid_mat = white->clone( VALID_MAT_NAME );

  pass = valid_mat->getTechnique(0)->getPass(0);

  pass->setPolygonMode( Ogre::PM_WIREFRAME );
  pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
  pass->setDepthWriteEnabled(false);
  pass->setDepthCheckEnabled(false);
  pass->setLightingEnabled(true);
  pass->setCullingMode( Ogre::CULL_NONE );
  pass->setAmbient(0.0, 0.0, 0.0);
  pass->setDiffuse(0.0, 0.0, 0.0, 0.3);
  pass->setSpecular(0.0, 0.0, 0.0, 0.3);

  pass->setSelfIllumination( 0.0, 0.9, 0.0 );

  // material for valid models when selected
  pass = valid_mat->clone( VALID_SEL_MAT_NAME )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 0.4, 1.0, 0.4 );

  // material for invalid models (wrong recognition)
  pass = valid_mat->clone( INVALID_MAT_NAME )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 0.9, 0.0, 0.0 );

  // material for invalid models (wrong recognition)
  pass = valid_mat->clone( INVALID_SEL_MAT_NAME )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 1.0, 0.4, 0.4 );

  pass->_dirtyHash();
}


void ObjectRecognitionRvizUI::parseMeshes(const std::vector<interactive_perception_msgs::ModelHypothesisList> &model_hyp_list)
{
  ROS_ASSERT( mesh_switchers_.size() == 0 );

  //get all the meshes from the database
  int num_models = model_hyp_list.size();
  mesh_switchers_.resize(num_models);
  for ( int m=0; m<num_models; ++m )
  {
    const std::vector<interactive_perception_msgs::ModelHypothesis> &hyp_list =
        model_hyp_list[m].hypotheses;

    int num_hyp = hyp_list.size();
    mesh_switchers_[m] = new MeshObjectSwitcher( VALID_MAT_NAME, VALID_SEL_MAT_NAME, INVALID_MAT_NAME, INVALID_SEL_MAT_NAME );

    if ( hyp_list.size() == 0 || hyp_list[0].mesh.triangles.size() == 0 )
    {
      continue;
    }

    for ( int h=0; h<num_hyp; ++h )
    {
      const shape_msgs::Mesh& mesh = hyp_list[h].mesh;
      const geometry_msgs::Pose pose = hyp_list[h].pose.pose;
#if 0
      ROS_INFO_STREAM( "Model " << m << ", hypothesis " << h
          << ") has " << mesh.triangles.size() << " triangles and "
          << mesh.vertices.size() << " vertices." );
      ROS_INFO_STREAM( " Position:" << pose.position.x << " "
          << pose.position.y << " " << pose.position.z <<
          ". Orientation: " << pose.orientation.w << " " << pose.orientation.x
          << " " << pose.orientation.y << " " << pose.orientation.z );
#endif
      MeshObject* mesh_object = new MeshObject( scene_manager_, scene_root_ );

      std::ostringstream s;
      s << "obj_rec model " << m << " hypothesis " << h;
      std::string name = s.str();

      mesh_object->loadMesh( name, mesh );
      mesh_object->setPose( pose );

      Ogre::Entity* entity = mesh_object->getEntity();
      entity->getUserObjectBindings().setUserAny( "model", Ogre::Any(m) );
      entity->setRenderQueueGroup(Ogre::RENDER_QUEUE_MAIN);

      mesh_switchers_[m]->addObject( mesh_object );
    }

    mesh_switchers_[m]->setValid( model_hyp_list[m].accept );
    mesh_switchers_[m]->setVisible( 0 );
  }
}


void ObjectRecognitionRvizUI::update(float wall_dt, float ros_dt)
{
  render_panel_->getRenderWindow()->update();
}


void ObjectRecognitionRvizUI::onRenderWindowMouseEvent( QMouseEvent* event )
{
  ROS_ASSERT( object_recognition_server_->isActive() );

  int x = event->x();
  int y = event->y();

  QSize size = render_panel_->size();

  //check if one object has been selected or clicked (using normalized screen coordinates x,y in [0-1]).
  Ogre::Ray mouse_ray = render_panel_->getCamera()->getCameraToViewportRay(x/float(size.width()), y/float(size.height()));
  ray_scene_query_->setRay( mouse_ray );

  for ( size_t m=0; m<mesh_switchers_.size(); ++m )
  {
    mesh_switchers_[m]->setSelected(false);
  }

  // Execute query
  ray_scene_query_->setSortByDistance(true);
  Ogre::RaySceneQueryResult& result = ray_scene_query_->execute();
  Ogre::RaySceneQueryResult::iterator iter = result.begin();

  int i=0;
  while( iter != result.end() )
  {
    Ogre::MovableObject* movable_object = iter->movable;
    if ( movable_object )
    {
      Ogre::Any model_any = movable_object->getUserObjectBindings().getUserAny( "model" );

      if ( !model_any.isEmpty() )
      {
        int model = Ogre::any_cast<int>(model_any);
        //ROS_INFO_STREAM( " model: " << model );

        if ( model >=0 && model < (int)mesh_switchers_.size() )
        {
          mesh_switchers_[model]->setSelected(true);

          if ( mesh_switchers_[model]->isValid() )
          {
            if (event->type() == QEvent::MouseButtonPress && event->button() == Qt::LeftButton)
            {
              mesh_switchers_[model]->next();
            }
            else if (event->type() == QEvent::MouseButtonPress && event->button() == Qt::RightButton)
            {
              mesh_switchers_[model]->setValid( false );
            }
          }
          else
          {
            if ( event->type() == QEvent::MouseButtonPress )
            {
              mesh_switchers_[model]->setValid( true );
            }
          }
        }
        break;
      }
    }

    i++;
    iter++;
  }
}


void ObjectRecognitionRvizUI::acceptButtonClicked()
{
  interactive_perception_msgs::ObjectRecognitionGuiResult result;

  result.selected_hypothesis_indices.resize( mesh_switchers_.size() );
  for ( unsigned i=0; i<mesh_switchers_.size(); ++i )
  {
    result.selected_hypothesis_indices[i] =
        mesh_switchers_[i]->isValid() ? mesh_switchers_[i]->getVisible() : -1;
  }

  object_recognition_server_->setSucceeded(result);
  cleanupAndHide();
}


void ObjectRecognitionRvizUI::cancelButtonClicked()
{
  object_recognition_server_->setAborted();
  cleanupAndHide();
}


void ObjectRecognitionRvizUI::startActionServer( ros::NodeHandle &node_handle )
{
  if ( object_recognition_server_ )
  {
    ROS_ERROR( "ObjectRecognitionGuiAction server already started!" );
    return;
  }

  //create non-threaded action server
  object_recognition_server_ =
    new actionlib::SimpleActionServer<interactive_perception_msgs::ObjectRecognitionGuiAction>( node_handle, "object_recognition_popup", false );

  object_recognition_server_->registerGoalCallback( boost::bind(&ObjectRecognitionRvizUI::acceptNewGoal, this) );
  object_recognition_server_->registerPreemptCallback( boost::bind(&ObjectRecognitionRvizUI::preempt, this) );

  object_recognition_server_->start();
}


void ObjectRecognitionRvizUI::stopActionServer()
{
  if ( !object_recognition_server_ )
  {
    ROS_ERROR("ObjectRecognitionGuiAction server cannot be stopped because it is not running.");
    return;
  }

  //if we're currently being active, we have to cancel everything, clean up & hide the window
  if ( object_recognition_server_->isActive() )
  {
    ROS_WARN("Aborting ObjectRecognitionGuiAction goal.");
    object_recognition_server_->setAborted();
    cleanupAndHide();
  }

  delete object_recognition_server_;
  object_recognition_server_ = 0;
}

void ObjectRecognitionRvizUI::acceptNewGoal()
{
  const interactive_perception_msgs::ObjectRecognitionGuiGoal::ConstPtr &goal = object_recognition_server_->acceptNewGoal();

  try {
    image_overlay_->setImage( goal->image );
    image_overlay_->update();
  } catch ( ... )
  {
    ROS_ERROR("Could not set the overlay image!");
    object_recognition_server_->setAborted();
    return;
  }

  rviz_interaction_tools::updateCamera( render_panel_->getCamera(), goal->camera_info );

  parseMeshes( goal->model_hypotheses );

  show();
}

void ObjectRecognitionRvizUI::preempt()
{
  object_recognition_server_->setPreempted();
  cleanupAndHide();
}




}