Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

GripperVisualMonitorTask.h

Go to the documentation of this file.
00001 #ifndef VISUAL_GRIPPER_MONITOR_TASK_H_
00002 #define VISUAL_GRIPPER_MONITOR_TASK_H_
00003 
00004 // INCLUDES
00005 // c++
00006 #include <deque>
00007 #include <iostream>
00008 
00009 // tekkotsu
00010 #include "DualCoding/ShapeAprilTag.h"
00011 #include "DualCoding/ShapeRoot.h"
00012 #include "DualCoding/VRmixin.h"
00013 
00014 // tekkodu
00015 #include "Kodu/PosOrientState.h"
00016 
00017 namespace Kodu {
00018     // tekkodu forward declarations
00019     class KoduWorld;
00020     class PerceptualTaskBase;
00021     class PosOrientState;
00022 
00023     //! A functor that will check if the object is still in the robot's gripper
00024     class MatchesObjectTagInGripper : public DualCoding::UnaryShapeRootPred {
00025     public:
00026         //! Constructor
00027         MatchesObjectTagInGripper()
00028           : DualCoding::UnaryShapeRootPred(),
00029             aprTagCentroid(),
00030             aprTagId(0)
00031         { }
00032 
00033         //! Copy constructor
00034         MatchesObjectTagInGripper(const MatchesObjectTagInGripper& kPred)
00035           : DualCoding::UnaryShapeRootPred(),
00036             aprTagCentroid(kPred.aprTagCentroid),
00037             aprTagId(kPred.aprTagId)
00038         {
00039             if (aprTagId < 0) std::cerr << "The object's april tag was not properly identified.\n";
00040         }
00041 
00042         //! Destructor
00043         ~MatchesObjectTagInGripper() {
00044             // no explicit implementation
00045         }
00046 
00047         //! Assignment operator
00048         MatchesObjectTagInGripper& operator=(const MatchesObjectTagInGripper& kPred) {
00049             if (this != &kPred) {
00050                 DualCoding::UnaryShapeRootPred::operator=(kPred);
00051                 aprTagCentroid = kPred.aprTagCentroid;
00052                 aprTagId = kPred.aprTagId;
00053             }
00054             return *this;
00055         }
00056 
00057         //! Checks if a shape matches the specified criteria
00058         bool operator()(const DualCoding::ShapeRoot&) const;
00059 
00060         friend class VisualGripperMonitorTask;
00061 
00062     private:
00063         static float const kMaxAprTagCentroidVariance;
00064         DualCoding::Point aprTagCentroid;   //!< The april tag's centroid (in local shape space)
00065         int aprTagId;                       //!< The object's april tag id number
00066     };
00067 
00068     class VisualGripperMonitorTask : public PerceptualTaskBase {
00069     public:
00070         //! Constructor
00071         VisualGripperMonitorTask(const DualCoding::ShapeRoot& kObjInGripper,
00072             const DualCoding::Shape<DualCoding::AprilTagData>& kObjAprilTag)
00073           : PerceptualTaskBase(PT_GRIPPER_VIS_MONITOR, ++idCount),
00074             taskPred(),
00075             objInGripper(kObjInGripper),
00076             lastSuccessfulState()
00077         {
00078             int tagId = -1;
00079             DualCoding::Point tagCen;
00080             
00081             if (!objInGripper.isValid()) {
00082                 std::cerr << "The objInGripper reference is not valid.\n";
00083             }
00084 
00085             if (kObjAprilTag.isValid()) {
00086                 tagId = kObjAprilTag->getTagID();
00087                 switch (kObjAprilTag->getRefFrameType()) {
00088                     case DualCoding::egocentric:
00089                         tagCen = kObjAprilTag->getCentroid();
00090                         break;
00091 
00092                     case DualCoding::allocentric:
00093                         tagCen = VRmixin::mapBuilder->importWorldToLocal(kObjAprilTag)->getCentroid();
00094                         break;
00095 
00096                     default:
00097                         std::cerr << "Unhandled reference frame type in VisualGripperMonitorTask.\n";
00098                         break;
00099                 }
00100             } else {
00101                 std::cerr << "April Tag reference is not valid!!!\n";
00102             }
00103             taskPred.aprTagId = tagId;
00104             taskPred.aprTagCentroid = tagCen;
00105             float orient = VRmixin::theAgent->getOrientation();
00106             lastSuccessfulState = PosOrientState(VRmixin::theAgent->getCentroid(), orient);
00107         }
00108 
00109         //! Copy constructor
00110         VisualGripperMonitorTask(const VisualGripperMonitorTask& kTask)
00111           : PerceptualTaskBase(kTask),
00112             taskPred(kTask.taskPred),
00113             objInGripper(kTask.objInGripper),
00114             lastSuccessfulState(kTask.lastSuccessfulState)
00115         {
00116             if (!objInGripper.isValid()) std::cerr << "The objInGripper reference is not valid.\n";
00117         }
00118 
00119         //! Destructor
00120         ~VisualGripperMonitorTask() {
00121             // no explicit implementation
00122         }
00123 
00124         //! Assignment operator
00125         VisualGripperMonitorTask& operator=(const VisualGripperMonitorTask& kTask) {
00126             if (this != &kTask) {
00127                 PerceptualTaskBase::operator=(kTask);
00128                 taskPred = kTask.taskPred;
00129                 objInGripper = kTask.objInGripper;
00130                 lastSuccessfulState = kTask.lastSuccessfulState;
00131             }
00132             return *this;
00133         }
00134 
00135         //! Checks if the VisualGripperMonitorTask can execute
00136         virtual bool canExecute(const KoduWorld&);
00137 
00138         //! Examines the results from the MapBuilder request to see if the object is still in the gripper
00139         virtual void examineTaskResults();
00140 
00141         //! Returns the object in the robot's gripper
00142         const DualCoding::ShapeRoot& getGripperObject() const;
00143 
00144         //! Returns the last successful state where the object was still in the robot gripper
00145         const PosOrientState& getLastSuccessfulState() const;
00146 
00147         //! Returns the MapBuilder request telling the robot where to look at the gripper
00148         virtual const DualCoding::MapBuilderRequest& getMapBuilderRequest();
00149 
00150         //! Returns the April Tag's centroid (relative to the body when the object is in the gripper)
00151         const DualCoding::Point& getTagCentroid() const;
00152 
00153         //! Returns the April Tag's id
00154         int getTagId() const;
00155 
00156         //! Relocates the object's tag (used after manipulation recovery)
00157         void relocateTagCentroid(const DualCoding::Point&);
00158 
00159         //! Checks whether a task is complete
00160         virtual bool taskIsComplete(const KoduWorld&);
00161 
00162     private:
00163         static unsigned int idCount;    //!< Used to generate id numbers for VisualGripperMonitorTask
00164         MatchesObjectTagInGripper taskPred;
00165         DualCoding::ShapeRoot objInGripper; //!< The reference to the object in the gripper
00166         PosOrientState lastSuccessfulState;
00167     };
00168 }
00169 
00170 #endif // VISUAL_GRIPPER_MONITOR_TASK_H_

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3