rviz_interaction_tools: rviz_interaction_tools: gripper.cpp Source File

00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "rviz_interaction_tools/gripper.h"
00031 
00032 
00033 #include <tf/transform_datatypes.h>
00034 
00035 #include <geometry_msgs/PoseStamped.h>
00036 
00037 #include <rviz/render_panel.h>
00038 #include <rviz/mesh_loader.h>
00039 
00040 #include <OGRE/OgreEntity.h>
00041 #include <OGRE/OgreLight.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 #include <OGRE/OgreSceneNode.h>
00044 #include <OGRE/OgreSubEntity.h>
00045 #include <OGRE/OgreTechnique.h>
00046 
00047 #include <rviz_interaction_tools/unique_string_manager.h>
00048 
00049 namespace rviz_interaction_tools
00050 {
00051 
00052 Gripper::Gripper( Ogre::SceneManager* scene_manager, Ogre::SceneNode* scene_root ) :
00053   gripper_transform_(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0.5))
00054 {
00055   scene_manager_ = scene_manager;
00056 
00057   std::string palm_string("package://pr2_description/meshes/gripper_v0/gripper_palm.dae");
00058   std::string proximal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger.dae");
00059   std::string distal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger_tip.dae");
00060 
00061   Ogre::MeshPtr palm_mesh = rviz::loadMeshFromResource(palm_string);
00062   Ogre::MeshPtr proximal_finger_mesh = rviz::loadMeshFromResource(proximal_finger_string);
00063   Ogre::MeshPtr distal_finger_mesh = rviz::loadMeshFromResource(distal_finger_string);
00064 
00065   if (!palm_mesh.get() || !proximal_finger_mesh.get() || !distal_finger_mesh.get()) 
00066   {
00067     ROS_ERROR("Failed to load resource");
00068   }
00069 
00070   rviz_interaction_tools::UniqueStringManager usm;
00071 
00072   resource_group_name_ = usm.unique("rviz_interaction_tools::Gripper");
00073   Ogre::ResourceGroupManager::getSingleton().createResourceGroup(resource_group_name_);
00074   Ogre::ResourceGroupManager::getSingleton().initialiseResourceGroup(resource_group_name_);
00075 
00076   Ogre::Entity* palm_entity = scene_manager->createEntity(usm.unique("palm_entity"), palm_string, resource_group_name_);
00077   Ogre::Entity* l_proximal_finger_entity = scene_manager->createEntity(usm.unique("l_proximal_finger_entity"),
00078                                                                         proximal_finger_string, resource_group_name_);
00079   Ogre::Entity* l_distal_finger_entity = scene_manager->createEntity(usm.unique("l_distal_finger_entity"),
00080                                                                       distal_finger_string, resource_group_name_);
00081   Ogre::Entity* r_proximal_finger_entity = scene_manager->createEntity(usm.unique("r_proximal_finger_entity"),
00082                                                                         proximal_finger_string);
00083   Ogre::Entity* r_distal_finger_entity = scene_manager->createEntity(usm.unique("r_distal_finger_entity"),
00084                                                                       distal_finger_string, resource_group_name_);
00085 
00086   entities_.push_back( palm_entity );
00087   entities_.push_back( l_proximal_finger_entity );
00088   entities_.push_back( l_distal_finger_entity );
00089   entities_.push_back( r_proximal_finger_entity );
00090   entities_.push_back( r_distal_finger_entity );
00091 
00092   material_.push_back( palm_entity->getSubEntity(0)->getMaterial()->clone( usm.unique("gripper_mat1"), true, resource_group_name_ ) );
00093   palm_entity->setMaterial(material_.back());
00094 
00095   material_.push_back( l_proximal_finger_entity->getSubEntity(0)->getMaterial()->clone( usm.unique("gripper_mat2"), true, resource_group_name_ ) );
00096   l_proximal_finger_entity->setMaterial(material_.back());
00097   r_proximal_finger_entity->setMaterial(material_.back());
00098 
00099   material_.push_back( l_distal_finger_entity->getSubEntity(0)->getMaterial()->clone( usm.unique("gripper_mat3"), true, resource_group_name_ ) );
00100   l_distal_finger_entity->setMaterial(material_.back());
00101   r_distal_finger_entity->setMaterial(material_.back());
00102 
00103   setColour(1,1,0,0.5);
00104 
00105   gripper_root_ = scene_root->createChildSceneNode();
00106 
00107   gripper_root_->setPosition(-0.1, 0.0, 0.5);
00108   gripper_root_->attachObject(palm_entity);
00109 
00110   l_proximal_finger_node_ = gripper_root_->createChildSceneNode();
00111   l_proximal_finger_node_->setPosition(0.07691, 0.01, 0);
00112   l_proximal_finger_node_->attachObject(l_proximal_finger_entity);
00113 
00114   l_distal_finger_node_ = l_proximal_finger_node_->createChildSceneNode();
00115   l_distal_finger_node_->setPosition(0.09137, 0.00495, 0);
00116   l_distal_finger_node_->attachObject(l_distal_finger_entity);
00117 
00118   Ogre::SceneNode* r_flip_node = gripper_root_->createChildSceneNode();
00119   r_flip_node->setOrientation(0, 1, 0, 0);
00120 
00121   r_proximal_finger_node_ = r_flip_node->createChildSceneNode();
00122   r_proximal_finger_node_->setPosition(0.07691, 0.01, 0);
00123   r_proximal_finger_node_->attachObject(r_proximal_finger_entity);
00124 
00125   r_distal_finger_node_ = r_proximal_finger_node_->createChildSceneNode();
00126   r_distal_finger_node_->setPosition(0.09137, 0.00495, 0);
00127   r_distal_finger_node_->attachObject(r_distal_finger_entity);
00128 }
00129 
00130 Gripper::~Gripper()
00131 {
00132   scene_manager_->destroySceneNode(l_proximal_finger_node_);
00133   scene_manager_->destroySceneNode(l_distal_finger_node_);
00134   scene_manager_->destroySceneNode(r_proximal_finger_node_);
00135   scene_manager_->destroySceneNode(r_distal_finger_node_);
00136   scene_manager_->destroySceneNode(gripper_root_);
00137   Ogre::ResourceGroupManager::getSingleton().destroyResourceGroup(resource_group_name_);
00138 }
00139 
00140 void Gripper::setColour( float r, float g, float b, float a )
00141 {
00142   for ( unsigned i=0; i<material_.size(); i++ )
00143   {
00144     Ogre::MaterialPtr material = material_[i];
00145 
00146     material->getBestTechnique()->getPass(0)->setDiffuse(r,g,b,a);
00147     material->getBestTechnique()->getPass(0)->setAmbient(r,g,b);
00148 
00149     if ( a < 0.9998 )
00150     {
00151       material->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00152       material->getTechnique(0)->setDepthWriteEnabled( false );
00153     }
00154     else
00155     {
00156       material->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
00157       material->getTechnique(0)->setDepthWriteEnabled( true );
00158     }
00159   }
00160 }
00161 
00162 void Gripper::setDepthCheckEnabled( bool enabled )
00163 {
00164   for ( unsigned i=0; i<material_.size(); i++ )
00165   {
00166     material_[i]->setDepthCheckEnabled( enabled );
00167   }
00168 }
00169 
00170 void Gripper::setRenderQueueGroup( unsigned group )
00171 {
00172   for ( unsigned i=0; i<entities_.size(); i++ )
00173   {
00174     entities_[i]->setRenderQueueGroup(group);
00175   }
00176 }
00177 
00178 void Gripper::setVisible( bool visible )
00179 {
00180   gripper_root_->setVisible(visible);
00181 }
00182 
00183 void Gripper::setGripperAngle(float angle)
00184 {
00185   gripper_angle_ = angle;
00186 
00187   l_proximal_finger_node_->resetOrientation();
00188   l_proximal_finger_node_->roll(Ogre::Radian(angle));
00189   l_distal_finger_node_->resetOrientation();
00190   l_distal_finger_node_->roll(Ogre::Radian(-angle));
00191 
00192   r_proximal_finger_node_->resetOrientation();
00193   r_proximal_finger_node_->roll(Ogre::Radian(angle));
00194   r_distal_finger_node_->resetOrientation();
00195   r_distal_finger_node_->roll(Ogre::Radian(-angle));
00196 }
00197 
00198 void Gripper::setPosition( Ogre::Vector3 &position )
00199 {
00200   gripper_root_->setPosition(position);
00201 }
00202 
00203 void Gripper::setOrientation( Ogre::Quaternion &orientation )
00204 {
00205   gripper_root_->setOrientation( orientation );
00206 }
00207 
00208 }