ObjectPicker.h
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 #ifndef OBJECTPICKER_H_
00014 #define OBJECTPICKER_H_
00015 
00016 #include "VirtualRangeSensor.h"
00017 #include "UWSimUtils.h"
00018 #include "URDFRobot.h"
00019 
00020 //Node tracker that updates the ray coordinates from the tracked node position, computes intersections and 'picks' nodes
00021 class ObjectPickerUpdateCallback : public IntersectorUpdateCallback
00022 {
00023   virtual void operator()(osg::Node *node, osg::NodeVisitor *nv)
00024   {
00025     osg::Matrixd mStart, mEnd;
00026     mStart = osg::computeLocalToWorld(nv->getNodePath());
00027     traverse(node, nv);
00028 
00029     //update ray and compute intersections. Checks intersections along X axis of the local frame
00030     mEnd = mStart;
00031     mEnd.preMultTranslate(osg::Vec3d(range, 0, 0));
00032 
00033     intersector->reset();
00034     intersector->setStart(mStart.getTrans());
00035     intersector->setEnd(mEnd.getTrans());
00036 
00037     root->accept(intersectVisitor);
00038 
00039     if (intersector->containsIntersections() && !picked)
00040     {
00041       osgUtil::LineSegmentIntersector::Intersection intersection = intersector->getFirstIntersection();
00042       osg::Vec3d worldIntPoint = intersection.getWorldIntersectPoint();
00043       distance_to_obstacle = (worldIntPoint - mStart.getTrans()).length();
00044       impact = intersection.nodePath;
00045 
00046       //search for catchable objects in nodepath
00047       for (osg::NodePath::iterator i = impact.begin(); i != impact.end(); ++i)
00048       {
00049         osg::ref_ptr<NodeDataType> data = dynamic_cast<NodeDataType*>(i[0]->getUserData());
00050         if (data != NULL && data->catchable)
00051         {
00052 
00053           std::cerr << "Picking object up." << std::endl;
00054           //physics: set static object flag
00055           if (data->rigidBody)
00056             data->rigidBody->setCollisionFlags(
00057                 data->rigidBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
00058 
00059           //add link to kinematic chain
00060           urdf->addToKinematicChain(i[0], data->rigidBody);
00061 
00062           osg::Node * objectTransf = i[0]->getParent(0)->getParent(0); //Object->linkBaseTransform->transform
00063 
00064           //Get coordinates to change them when changing position in graph
00065           boost::shared_ptr<osg::Matrix> originalpos = getWorldCoords(objectTransf);
00066           boost::shared_ptr<osg::Matrix> hand = getWorldCoords(trackNode);
00067           hand->invert(*hand);
00068 
00069           //ADD node in hand, remove object from original position.
00070           trackNode->asTransform()->addChild(objectTransf);
00071           objectTransf->getParent(0)->asGroup()->removeChild(objectTransf);
00072 
00073           osg::Matrixd matrix = *originalpos * *hand;
00074           objectTransf->asTransform()->asMatrixTransform()->setMatrix(matrix);
00075 
00076           picked = true;
00077         }
00078       }
00079     }
00080     else if (!picked)
00081       distance_to_obstacle = range;
00082   }
00083 public:
00084   osg::NodePath impact;
00085   osg::Node *trackNode;
00086   boost::shared_ptr<URDFRobot> urdf;
00087   bool picked;
00088 
00089   ObjectPickerUpdateCallback(osg::Node *trackNode, double range, bool visible, osg::Node *root,
00090                              boost::shared_ptr<URDFRobot> urdf) :
00091       IntersectorUpdateCallback(range, visible, root)
00092   {
00093     this->trackNode = trackNode;
00094     picked = false;
00095     this->urdf = urdf;
00096   }
00097 };
00098 
00099 class ObjectPicker : public VirtualRangeSensor
00100 {
00101 public:
00102   ObjectPicker(std::string name, osg::Node *root, osg::Node *trackNode, double range, bool visible,
00103                boost::shared_ptr<URDFRobot> urdf,unsigned int mask);
00104   ObjectPicker();
00105 
00106   void init(std::string name, osg::Node *root, osg::Node *trackNode, double range, bool visible,
00107             boost::shared_ptr<URDFRobot> urdf,unsigned int mask);
00108 };
00109 
00110 #endif


uwsim
Author(s): Mario Prats , Javier Perez
autogenerated on Fri Aug 28 2015 13:28:58