object_selection_frame.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 "pr2_interactive_manipulation/object_selection_frame.h"

#include "rviz_interaction_tools/image_tools.h"

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

#include <ogre_tools/point_cloud.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/visualization_manager.h>

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

#include "household_objects_database_msgs/GetModelMesh.h"

using namespace rviz_interaction_tools;
using namespace household_objects_database_msgs;

static const std::string mat_base_name = "object_selection_frame";

namespace pr2_interactive_manipulation
{

ObjectSelectionFrame::ObjectSelectionFrame(rviz::VisualizationManager *visualization_manager, wxWindow *parent) :
    ObjectSelectionFrameBase( parent ),
    selected_object_(-1),
    action_state_(IDLE)
{
  UniqueStringManager usm;

  //construct basic ogre scene
  scene_manager_ = Ogre::Root::getSingleton().createSceneManager( Ogre::ST_GENERIC, usm.unique("ObjectSelectionFrame") );
  //scene_manager_->showBoundingBoxes(true);

  scene_root_ = scene_manager_->getRootSceneNode()->createChildSceneNode();

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

  createRenderPanel( visualization_manager );

  createMaterials();

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

  Hide();
}


ObjectSelectionFrame::~ObjectSelectionFrame()
{
  cleanupAndHide();

  scene_manager_->destroyQuery( ray_scene_query_ );

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

  delete render_panel_;
  delete image_overlay_;
}


void ObjectSelectionFrame::cleanupAndHide()
{
  meshes_.clear();
  scene_root_->removeAndDestroyAllChildren();
  Hide();
}


void ObjectSelectionFrame::createRenderPanel( rviz::VisualizationManager *visualization_manager )
{
  //put a render panel into the window
  render_panel_ = new rviz::RenderPanel(this, false);

  render_panel_->SetSize( panel_->GetRect() );

  // notify us when the user does something with the mouse
  render_panel_->Connect( wxEVT_MOTION,
                          wxMouseEventHandler( ObjectSelectionFrame::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_LEFT_DOWN,
                          wxMouseEventHandler( ObjectSelectionFrame::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_RIGHT_DOWN,
                          wxMouseEventHandler( ObjectSelectionFrame::onRenderWindowMouseEvents ), NULL, this );

  render_panel_->createRenderWindow();
  render_panel_->initialize(scene_manager_, visualization_manager);

  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 ObjectSelectionFrame::createMaterials()
{
  Ogre::MaterialPtr white = Ogre::MaterialManager::getSingleton().getByName("BaseWhiteNoLighting");

  if ( !Ogre::MaterialManager::getSingleton().getByName("object_selection_normal").isNull() )
  {
    //materials already exist
    return;
  }

  Ogre::Pass *pass;

  // material for valid models
  Ogre::MaterialPtr base_material = white->clone( mat_base_name+"_mesh_normal" );

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

  pass->setPolygonMode( Ogre::PM_WIREFRAME );
  pass->setPointSize( 2.0 );
  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.9, 0.0, 0.0 );

  pass = base_material->clone(  mat_base_name+"_point_normal" )->getTechnique(0)->getPass(0);
  pass->setPolygonMode( Ogre::PM_POINTS );

  // material for valid models when highlighted
  pass = base_material->clone( mat_base_name+"_mesh_highlighted" )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 1.0, 0.4, 0.4 );

  pass = base_material->clone(  mat_base_name+"_point_highlighted" )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 1.0, 0.4, 0.4 );
  pass->setPolygonMode( Ogre::PM_POINTS );

  // material for valid models when selected
  pass = base_material->clone( mat_base_name+"_mesh_selected" )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 0.4, 1.0, 0.4 );

  pass = base_material->clone( mat_base_name+"_point_selected" )->getTechnique(0)->getPass(0);
  pass->setSelfIllumination( 0.4, 1.0, 0.4 );
  pass->setPolygonMode( Ogre::PM_POINTS );
}


void ObjectSelectionFrame::setData( pr2_interactive_object_detection::GraspableObjectListConstPtr received_objects )
{
  if ( meshes_.size() != 0 )
  {
    cleanupAndHide();
  }

  selected_object_ = -1;

  const geometry_msgs::Pose &pose = received_objects->reference_to_camera;
  scene_root_->setPosition( pose.position.x, pose.position.y, pose.position.z );
  scene_root_->setOrientation( pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z );

  for ( unsigned o=0; o<received_objects->meshes.size(); o++ )
  {
    if ( received_objects->meshes[o].vertices.size() > 0 )
    {
      addMesh( o, received_objects->meshes[o], received_objects->graspable_objects[o].potential_models[0].pose.pose );
    }
    else
    {
      addCluster( o, received_objects->graspable_objects[o].cluster );
    }
  }

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

  image_overlay_->setImage( received_objects->image );
  image_overlay_->update();

  action_state_ = ACTIVE;
  Show();
}


void ObjectSelectionFrame::addCluster( int object_index, const sensor_msgs::PointCloud &cluster )
{
  UniqueStringManager usm;

  MeshObject *mesh_object = new MeshObject( scene_manager_, scene_root_ );

  mesh_object->loadPoints( usm.unique("object_selection_frame_object_"), cluster.points );
  mesh_object->setMaterialName(mat_base_name+"_point_normal");

  Ogre::Entity* entity = mesh_object->getEntity();
  entity->getUserObjectBindings().setUserAny( "object", Ogre::Any(object_index) );
  entity->getUserObjectBindings().setUserAny( "type", Ogre::Any(std::string("point")) );
  entity->setRenderQueueGroup(Ogre::RENDER_QUEUE_MAIN );

  meshes_[object_index] = boost::shared_ptr<rviz_interaction_tools::MeshObject>( mesh_object );
}


void ObjectSelectionFrame::addMesh( int object_index, const geometric_shapes_msgs::Shape &mesh,
    const geometry_msgs::Pose &pose )
{
  UniqueStringManager usm;

  MeshObject *mesh_object = new MeshObject( scene_manager_, scene_root_ );

  ROS_INFO_STREAM( "Object " << object_index << " has "
      << mesh.triangles.size()/3 << " 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 );

  mesh_object->loadMesh( usm.unique("object_selection_frame_object_"), mesh );
  mesh_object->setPose( pose );
  mesh_object->setMaterialName(mat_base_name+"_mesh_normal");

  Ogre::Entity* entity = mesh_object->getEntity();
  entity->getUserObjectBindings().setUserAny( "object", Ogre::Any(object_index) );
  entity->getUserObjectBindings().setUserAny( "type", Ogre::Any(std::string("mesh")) );
  entity->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY);

  meshes_[object_index] = boost::shared_ptr<rviz_interaction_tools::MeshObject>( mesh_object );
}


void ObjectSelectionFrame::update()
{
  render_panel_->getRenderWindow()->update();
}


void ObjectSelectionFrame::onRenderWindowMouseEvents( wxMouseEvent& event )
{
  int x = event.GetX();
  int y = event.GetY();

  //convert to coordinates in the original image resolution
  wxSize size = render_panel_->GetSize();
  x = floor(x * image_overlay_->getWidth() / size.GetWidth() );
  y = floor(y * image_overlay_->getHeight() / size.GetHeight() );

  //check if one object has been selected or clicked
  Ogre::Ray mouse_ray = render_panel_->getCamera()->getCameraToViewportRay(x/float(size.GetWidth()), y/float(size.GetHeight()));
  ray_scene_query_->setRay( mouse_ray );

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

  unsigned highlighted_object = -1;

  //look for object under mouse
  while( iter != result.end() )
  {
    Ogre::MovableObject* movable_object = iter->movable;
    if ( movable_object )
    {
      Ogre::Any model_any = movable_object->getUserObjectBindings().getUserAny( "object" );
      if ( !model_any.isEmpty() )
      {
        highlighted_object = Ogre::any_cast<int>(model_any);
        if ( event.ButtonDown(wxMOUSE_BTN_LEFT) )
        {
          selected_object_ = highlighted_object;
        }
        break;
      }
    }
    iter++;
  }

  std::map< unsigned, boost::shared_ptr<rviz_interaction_tools::MeshObject> >::iterator mesh_it = meshes_.begin();
  while( mesh_it != meshes_.end() )
  {
    Ogre::Any type_any = mesh_it->second->getEntity()->getUserObjectBindings().getUserAny( "type" );
    std::string type;
    if ( !type_any.isEmpty() )
    {
      type = Ogre::any_cast<std::string>(type_any);
    }
    else
    {
      ROS_ERROR( "Couldn't determine type of mesh object." );
    }

    if ( mesh_it->first == selected_object_ )
    {
      mesh_it->second->setMaterialName(mat_base_name + "_" + type + "_selected");
    }
    else if ( mesh_it->first == highlighted_object )
    {
      mesh_it->second->setMaterialName(mat_base_name + "_" + type + "_highlighted");
    }
    else
    {
      mesh_it->second->setMaterialName(mat_base_name + "_" + type + "_normal");
    }
    mesh_it++;
  }
}


void ObjectSelectionFrame::acceptButtonClicked( wxCommandEvent& )
{
  action_state_ = ACCEPTED;
  cleanupAndHide();
}


void ObjectSelectionFrame::cancelButtonClicked( wxCommandEvent& )
{
  action_state_ = CANCELED;
  cleanupAndHide();
}

}

---

pr2_interactive_manipulation
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 11 09:11:03 2013