GetObjectTask.java
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011, 2012
00004  *
00005  * School of Engineering, Cardiff University, UK
00006  *
00007  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00008  *
00009  * Project name: srs EU FP7 (www.srs-project.eu)
00010  * ROS stack name: srs
00011  * ROS package name: srs_knowledge
00012  * Description: 
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *
00016  * @author Ze Ji, email: jiz1@cf.ac.uk
00017  *
00018  * Date of creation: Oct 2011:
00019  * ToDo: 
00020  *
00021  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *       * Redistributions of source code must retain the above copyright
00027  *         notice, this list of conditions and the following disclaimer.
00028  *       * Redistributions in binary form must reproduce the above copyright
00029  *         notice, this list of conditions and the following disclaimer in the
00030  *         documentation and/or other materials provided with the distribution.
00031  *       * Neither the name of the school of Engineering, Cardiff University nor 
00032  *         the names of its contributors may be used to endorse or promote products 
00033  *         derived from this software without specific prior written permission.
00034  *
00035  * This program is free software: you can redistribute it and/or modify
00036  * it under the terms of the GNU Lesser General Public License LGPL as 
00037  * published by the Free Software Foundation, either version 3 of the 
00038  * License, or (at your option) any later version.
00039  * 
00040  * This program is distributed in the hope that it will be useful,
00041  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00042  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00043  * GNU Lesser General Public License LGPL for more details.
00044  * 
00045  * You should have received a copy of the GNU Lesser General Public 
00046  * License LGPL along with this program. 
00047  * If not, see <http://www.gnu.org/licenses/>.
00048  *
00049  ****************************************************************/
00050 
00051 package org.srs.srs_knowledge.task;
00052 
00053 import java.io.*;
00054 import java.util.StringTokenizer;
00055 import java.util.ArrayList;
00056 import java.util.Iterator;
00057 import ros.pkg.srs_knowledge.msg.*;
00058 import ros.pkg.geometry_msgs.msg.Pose2D;
00059 import ros.pkg.geometry_msgs.msg.Pose;
00060 import org.srs.srs_knowledge.knowledge_engine.*;
00061 import com.hp.hpl.jena.rdf.model.*;
00062 import com.hp.hpl.jena.query.QueryExecutionFactory;
00063 import com.hp.hpl.jena.query.ResultSet;
00064 import com.hp.hpl.jena.query.QueryExecution;
00065 import com.hp.hpl.jena.query.QuerySolution;
00066 import com.hp.hpl.jena.ontology.Individual;
00067 import org.srs.srs_knowledge.task.*;
00068 
00069 import ros.pkg.srs_symbolic_grounding.srv.*;
00070 import ros.pkg.srs_symbolic_grounding.msg.*;
00071 
00072 import ros.*;
00073 import ros.communication.*;
00074 
00075 import org.json.simple.JSONArray;
00076 import org.json.simple.JSONObject;
00077 import org.json.simple.JSONValue;
00078 import org.json.simple.parser.ParseException;
00079 import org.json.simple.parser.JSONParser;
00080 
00081 import org.srs.srs_knowledge.utils.*;
00082 
00083 public class GetObjectTask extends org.srs.srs_knowledge.task.Task
00084 {
00085     /*
00086     public static enum GraspType {
00087         JUST_GRASP, MOVE_AND_GRASP
00088     }
00089     */
00090     
00091     private ConfigInfo.GraspType graspType = ConfigInfo.GraspType.MOVE_AND_GRASP;
00092 
00093     public GetObjectTask(String targetContent) 
00094     {   
00095         // this.init(taskType, targetContent, userPose);
00096         this.initTask(targetContent);
00097         System.out.println("MOVE_AND_GRASP");
00098     }
00099 
00100     public GetObjectTask(String targetContent, ConfigInfo.GraspType graspMode) 
00101     {   
00102         this.graspType = graspMode;
00103         // this.init(taskType, targetContent, userPose);
00104         if (this.initTask(targetContent)) {
00105             if(graspMode == ConfigInfo.GraspType.MOVE_AND_GRASP)  {
00106                 System.out.println("MOVE_AND_GRASP  " + targetContent);
00107             }
00108             else {
00109                 System.out.println("JUST_GRASP  " + targetContent);
00110             }
00111         }
00112         else {
00113             System.out.println("Task not constructed successfully");
00114         }
00115     }
00116 
00117     public void setGraspMode(ConfigInfo.GraspType graspMode) {
00118         this.graspType = graspMode;
00119         if(graspMode == ConfigInfo.GraspType.MOVE_AND_GRASP)  {
00120             System.out.println("MOVE_AND_GRASP  ");
00121         }
00122         else {
00123             System.out.println("JUST_GRASP  ");
00124         }
00125     }
00126 
00127     @Override
00128     protected boolean constructTask() {
00129         return createGetObjectTask();
00130     }
00131     
00132     private boolean createGetObjectTask() {
00133         /*
00134         // query for tables
00135         // move to tables (near -- use grounding)
00136         // detect milk
00137         // grap milk
00138         // back to user
00139         */
00140         System.out.println("Create New GET OBJECT Task --- ");
00141 
00142         try {
00143             //ArrayList<String> workspaces = OntoQueryUtil.getWorkspaceNamesOfObject(this.targetContent, this.ontoQueryUtil.getObjectNameSpace(), this.ontoQueryUtil.getGlobalNameSpace(), this.ontoDB);
00144             //      workspaces = OntoQueryUtil.getWorkspaceOfObject(this.targetContent, OntoQueryUtilthis.ontoQueryUtil.getObjectNameSpace(), this.ontoQueryUtil.getGlobalNameSpace(), KnowledgeEngine.ontoDB);
00145             workspaces = OntoQueryUtil.getWorkspaceOfObject(this.targetContent, OntoQueryUtil.ObjectNameSpace, OntoQueryUtil.GlobalNameSpace, KnowledgeEngine.ontoDB);
00146         }
00147         catch(Exception e) {
00148             System.out.println("--> " + e.getMessage() + "\n" + e.toString());
00149             return false;
00150         }
00151         
00152         for(Individual u : workspaces) {
00153             //System.out.println(u.getLocalName());
00154             try{
00155                 //System.out.println("Created HLActionSeq ");
00156                 HighLevelActionSequence subSeq = createSubSequenceForSingleWorkspace(u);
00157                 if (subSeq != null)
00158                     allSubSeqs.add(subSeq);
00159                 else System.out.println("SubSequence is null - probably no valid scanning position is given by symbolic grounding service");
00160             }
00161             catch(RosException e) {
00162                 System.out.println("ROSEXCEPTION -- when calling symbolic grounding for scanning positions.  \t" + e.getMessage() + "\t" + e.toString());
00163             }
00164             catch(Exception e) {
00165                 System.out.println(e.getMessage());
00166                 System.out.println(e.toString());
00167             }
00168         }       
00169         
00170         if(allSubSeqs.size() > 0) {
00171             currentSubAction = 0;
00172             return true;
00173         }
00174         else {
00175             currentSubAction = -1; // no sub_highlevel_action in list
00176             return false;
00177         }
00178     }
00179 
00180     
00181     private HighLevelActionSequence createSubSequenceForSingleWorkspace(Individual workspace) throws RosException, Exception {
00182         HighLevelActionSequence actionList = new HighLevelActionSequence();
00183         
00184         // create MoveAndDetectionActionUnit
00185         //SRSFurnitureGeometry spatialInfo = new SRSFurnitureGeometry();
00186         ros.pkg.srs_msgs.msg.SRSSpatialInfo spatialInfo = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00187         
00188         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00189         
00190         spatialInfo.pose.position.x = stm.getFloat();
00191         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00192         spatialInfo.pose.position.y = stm.getFloat();
00193         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00194         spatialInfo.pose.position.z = stm.getFloat();
00195                 
00196         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00197         spatialInfo.w = stm.getFloat();
00198         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00199         spatialInfo.h = stm.getFloat();
00200         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00201         spatialInfo.l = stm.getFloat();
00202         
00203         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00204         spatialInfo.pose.orientation.w = stm.getFloat();
00205         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00206         spatialInfo.pose.orientation.x = stm.getFloat();
00207         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00208         spatialInfo.pose.orientation.y = stm.getFloat();
00209         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00210         spatialInfo.pose.orientation.z = stm.getFloat();
00211         
00212         // call symbolic grounding service for target pose
00213         ArrayList<Pose2D> posList;
00214         try {
00215             posList = calculateScanPositions(spatialInfo);
00216             System.out.println("Scan Poses: " + posList.size() + "  " + workspace.asResource().getLocalName());
00217         }
00218         catch(RosException e) {
00219             System.out.println(e.toString()); 
00220             System.out.println(e.getMessage());
00221             throw e;
00222         }
00223 
00224         Iterator<Individual> itInd = KnowledgeEngine.ontoDB.getInstancesOfClass(OntoQueryUtil.GlobalNameSpace + this.targetContent);
00225         int hhid = -1000;
00226         if(itInd.hasNext()) { 
00227             stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "houseHoldObjectID",  (Individual)itInd.next());
00228             hhid = OntoQueryUtil.getIntOfStatement(stm);
00229         }
00230 
00231         // TODO:
00232         MoveAndDetectionActionUnit mdAction = new MoveAndDetectionActionUnit(posList, targetContent, hhid, workspace.asResource().getLocalName());
00233         
00234         // create MoveAndGraspActionUnit
00235         HighLevelActionUnit mgAction = null;
00236         if(this.graspType == ConfigInfo.GraspType.MOVE_AND_GRASP) {
00237             mgAction = new MoveAndGraspActionUnit(null, targetContent, hhid, "side", workspace.asResource().getLocalName());
00238         }
00239         else if(this.graspType == ConfigInfo.GraspType.JUST_GRASP) {
00240             mgAction = new JustGraspActionUnit(targetContent, hhid, "side", workspace.asResource().getLocalName());
00241         }
00242         else {
00243             mgAction = new MoveAndGraspActionUnit(null, targetContent, hhid, "side", workspace.asResource().getLocalName());
00244         }
00245 
00246         // create PutOnTrayActionUnit
00247         PutOnTrayActionUnit trayAction = new PutOnTrayActionUnit("side");
00248 
00249         // FoldArmActionUnit
00250         //FoldArmActionUnit foldArmAction = new FoldArmActionUnit();
00251 
00252         //create BackToUserActionUnit
00253         
00254         //Pose2D posUser = OntoQueryUtil.parsePose2D(userPose);
00255         //MoveActionUnit mau = new MoveActionUnit(posUser);
00256         
00257         // create FinishActionUnit
00258         FinishActionUnit fau = new FinishActionUnit(true);
00259         //MoveActionUnit mau1 = new MoveActionUnit(OntoQueryUtil.parsePose2D("home"));
00260         //actionList.appendHighLevelAction(mau1);
00261 
00262         actionList.appendHighLevelAction(mdAction);
00263         actionList.appendHighLevelAction(mgAction);
00264         actionList.appendHighLevelAction(trayAction);
00265         //actionList.appendHighLevelAction(foldArmAction);
00266         //actionList.appendHighLevelAction(mau);
00267         actionList.appendHighLevelAction(fau);
00268                 
00269         System.out.println("ActionList size is " + actionList.getSizeOfHighLevelActionList());
00270         
00271         // set parameters for the workspace information
00272         actionList.setParameter("workspaceURI", workspace.asResource().getURI());
00273         actionList.setParameter("workspace", workspace.asResource().getLocalName());
00274         actionList.setParameter("workspaceNameSpace", workspace.asResource().getNameSpace());
00275         
00276         return actionList;
00277     }
00278 
00279     // TODO:   NOT COMPLETED... 
00280     @Override
00281     public CUAction getNextCUActionNew(boolean stateLastAction, String jsonFeedback) {
00282         System.out.println("===> Get Next CUACTION -- from GetObjectTask.java");
00283         CUAction ca = new CUAction();
00284         if(allSubSeqs.size() == 0 ) {
00285             System.out.println("Sequence size is zero");
00286             return null;   // ??? 
00287         }
00288         if(currentSubAction >= 0 && currentSubAction < allSubSeqs.size()) {
00289             // get the current SubActionSequence item
00290             System.out.println("Sequence size is " + allSubSeqs.size());
00291             HighLevelActionSequence subActSeq = allSubSeqs.get(currentSubAction);
00292             HighLevelActionUnit highAct = subActSeq.getCurrentHighLevelActionUnit();
00293 
00294             // decide if the current SubActionSequence is finished or stuck somewhere? 
00295             // if successfully finished, then finished
00296             // if stuck (fail), move to the next subActionSequence
00297             if(highAct != null) {
00298 
00299 
00300                 // TODO:
00301                 updateDBObjectPose();
00302                                 
00303                 int ni = highAct.getNextCUActionIndex(stateLastAction); 
00304 
00305                 switch(ni) {
00306                 case HighLevelActionUnit.COMPLETED_SUCCESS:
00307                     System.out.println("COMPLETED_SUCCESS");
00308                     lastStepActUnit = highAct;
00309 
00310                     CUAction retact = null;
00311                     try {
00312                         retact = handleSuccessMessageNew(new ActionFeedback(jsonFeedback));
00313                     }
00314                     catch(ParseException pe) {
00315                         System.out.println(pe.toString());
00316                         return null;
00317                     }
00318 
00319                     return retact; 
00320                 case HighLevelActionUnit.COMPLETED_FAIL:
00321                     // The whole task finished (failure). 
00322                     lastStepActUnit = null;
00323                     System.out.println("COMPLETED_FAIL");
00324                     return handleFailedMessage();
00325                 case HighLevelActionUnit.INVALID_INDEX:
00326                     // The whole task finished failure. Should move to a HighLevelActionUnit in subActSeq of finsihing
00327                     lastStepActUnit = null;
00328                     System.out.println("INVALID_INDEX");
00329                     //currentSubAction++;
00330                     return handleFailedMessage();
00331                 default: 
00332                     System.out.println(highAct.getActionType());
00333                     if(!highAct.ifParametersSet()) {
00334                         System.out.println("Parameters not set");
00335                         lastStepActUnit = null;
00336                         return handleFailedMessage();
00337                     }
00338                     ca = highAct.getCUActionAt(ni);
00339 
00340                     // maybe actionUnit is empty
00341                     if (ca == null)  {
00342                         return handleFailedMessage();
00343                     }
00344                     
00345                     // since it is going to use String list to represent action info. So cation type is always assumed to be generic, hence the first item in the list actionInfo should contain the action type information...
00346                     // WARNING: No error checking here
00347                     //lastActionType = ca.generic.actionInfo.get(0);
00348                     lastActionType = (String)(SRSJSONParser.decodeJsonActionInfo(ca.generic.jsonActionInfo).get("action"));
00349                     return ca;
00350                 } 
00351             }
00352             else {
00353                 return null;
00354             }       
00355             // or if still pending CUAction is available, return CUAction
00356         }
00357         else if (currentSubAction == -1) {
00358         }
00359 
00360         return ca;
00361     }
00362 
00363     private CUAction handleFailedMessage() {
00364 
00365         // 
00366         updateTargetOfFailedAct();
00367 
00368         currentSubAction++;
00369         
00370         //System.out.println("HANDLE FAILED MESSAGE.... CURRENTSUBACTION IS AT:  " + currentSubAction);
00371         
00372         if(currentSubAction >= allSubSeqs.size()) {
00373             return null;
00374         }
00375         HighLevelActionSequence nextHLActSeq = allSubSeqs.get(currentSubAction);
00376                 
00377         //      if(nextHLActSeq.hasNextHighLevelActionUnit()) {
00378         HighLevelActionUnit nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00379         if(nextHighActUnit != null) {
00380             int tempI = nextHighActUnit.getNextCUActionIndex(true); 
00381 
00382             CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00383             
00384             while (ca == null) {
00385                 
00386                 System.out.println("CUACTION IS NULL....... Keep trying   " + currentSubAction);
00387                 currentSubAction++;
00388                 if(currentSubAction >= allSubSeqs.size()) {
00389                     return null;
00390                 }
00391                 nextHLActSeq = allSubSeqs.get(currentSubAction);
00392                 nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00393                 
00394                 tempI = nextHighActUnit.getNextCUActionIndex(true);
00395                 ca = nextHighActUnit.getCUActionAt(tempI); 
00396             }
00397 
00398             
00399             // TODO: COULD BE DONE RECURSIVELY. BUT TOO COMPLEX UNNECESSARY AND DIFFICULT TO DEBUG. 
00400             // SO STUPID CODE HERE
00401             /*
00402             if(tempI == HighLevelActionUnit.COMPLETED_SUCCESS || tempI == HighLevelActionUnit.COMPLETED_FAIL || tempI == HighLevelActionUnit.INVALID_INDEX) {
00403                 CUAction ca = new CUAction();
00404                 ca.status = -1;
00405                 return ca;
00406             }
00407             else {
00408                 //System.out.println("GET NEXT CU ACTION AT:  " + tempI);
00409                 CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00410 
00411                 while (ca == null) {
00412 
00413                     System.out.println("CUACTION IS NULL....... Keep trying   " + currentSubAction);
00414                     currentSubAction++;
00415                     if(currentSubAction >= allSubSeqs.size()) {
00416                         return null;
00417                     }
00418                     nextHLActSeq = allSubSeqs.get(currentSubAction);
00419                     nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00420 
00421                     tempI = nextHighActUnit.getNextCUActionIndex(true);
00422                     ca = nextHighActUnit.getCUActionAt(tempI); 
00423                 }
00424             */
00425                 /*
00426                 if(ca == null) {
00427                 System.out.println("CUACTION IS NULL.......");
00428                 }*/
00429             return ca;
00430                 //}
00431         }
00432         
00433         return null;
00434     }
00435 
00439     private void updateTargetOfSucceededActNew(ActionFeedback fb) {
00440         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00441         HighLevelActionUnit currentActUnit = currentHLActSeq.getCurrentHighLevelActionUnit();
00442         if(currentActUnit.getActionType().equals("MoveAndDetection")) {
00443             //this.recentDetectedObject = ActionFeedback.toPose(fb);
00444             this.recentDetectedObject = fb.getDetectedObjectPose();
00445             BoundingBoxDim bbDim = InformationRetrieval.retrieveBoundingBoxInfo(OntoQueryUtil.GlobalNameSpace + this.targetContent);
00446             ros.pkg.srs_msgs.msg.SRSSpatialInfo spaObj = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00447             spaObj.l = bbDim.l;
00448             spaObj.h = bbDim.h;
00449             spaObj.w = bbDim.w;
00450 
00451             spaObj.pose = this.recentDetectedObject;
00452 
00453             // update the knowledge (post-processing)
00454             
00455             // if there exists one same object on the same workspace, update it --- simple solution
00456             //String ws = SpatialCalculator.workspaceHolding(spaObj);
00457             
00458             String neighbour = SpatialCalculator.nearestObject(spaObj.pose, OntoQueryUtil.GlobalNameSpace + this.targetContent);
00459             if(!neighbour.trim().equals("")) {
00460                 //System.out.println("Found neighbour of " + neighbour);
00461                 // update its pose
00462                 try{
00463                     OntoQueryUtil.updatePoseOfObject(spaObj.pose, OntoQueryUtil.GlobalNameSpace, neighbour.trim());
00464                     OntoQueryUtil.computeOnSpatialRelation();
00465                 }
00466                 catch(Exception e) {
00467                     System.out.println(e.getMessage());
00468                 }
00469             }
00470             
00471             // if there does not exist such an object, then insert a new one
00472             // bounding box can be obtained from HHDB
00473             // TODO
00474             
00475         }
00476         else if(currentActUnit.getActionType().equals("MoveAndGrasp")) {
00477             // look for the object at the pose 
00478             // update its relationship with the Robot, and remove its pose information
00479             //      Individual rob = KnowledgeEngine.ontoDB.getIndividual(OntoQueryUtil.ObjectNameSpace + OntoQueryUtil.RobotName);
00480             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00481             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00482                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00483                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00484             String queryString = "SELECT DISTINCT ?gripper WHERE { "
00485                 + "<" + mapNameSpace + OntoQueryUtil.RobotName + ">"
00486                 + " srs:hasPart ?gripper . " 
00487                 + " ?gripper a srs:RobotGripper . " 
00488                 + "}";
00489 
00490             try {
00491                 String targetObj = SpatialCalculator.nearestObject(this.recentDetectedObject, OntoQueryUtil.GlobalNameSpace + this.targetContent);
00492                 //System.out.println("TARGET OBJECT IS ::: " + targetObj);
00493                 if(!targetObj.trim().equals("")) {
00494                    
00495                     Pose tmpPose = new Pose();
00496                     tmpPose.position.x = -1000;
00497                     tmpPose.position.y = -1000;
00498                     tmpPose.position.z = -1000;
00499                     tmpPose.orientation.x = -1000;
00500                     tmpPose.orientation.y = -1000;
00501                     tmpPose.orientation.z = -1000;
00502                     tmpPose.orientation.w = -1000;
00503                     
00504                     // update its pose
00505                     try{
00506                         OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, targetObj.trim());
00507                         OntoQueryUtil.computeOnSpatialRelation();
00508                     }
00509                     catch(Exception e) {
00510                         System.out.println(e.getMessage());
00511                     }
00512                 }
00513                
00514                 
00515                 // OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, OntoQueryUtil.ObjectNameSpace, this.targetContent);
00516                 ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix + queryString);
00517                 if(rset.size() == 0) {
00518                     //System.out.println("<<<< NO GRIPPER INSTANCE FOUND >>>");   
00519                 }
00520                 else {
00521                     //System.out.println("<<<< FOUND GRIPPER INSTANCE >>>");
00522                     Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(targetObj);
00523                     
00524                     OntoQueryUtil.removeAllSubPropertiesOf(targetObj, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00525                     QuerySolution qs = rset.get(0);
00526 
00527                     String gripper = qs.get("gripper").toString();
00528                     //System.out.println("<<<<<  " + gripper + "  >>>>>");
00529 
00530                     Individual gripInd = KnowledgeEngine.ontoDB.getIndividual(gripper);
00531                     Property proExist = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00532                     
00533                     Property pro = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "grippedBy");
00534 
00535                     targetInd.setPropertyValue(pro, gripInd);
00536                 }
00537             }
00538             catch (Exception e) {
00539                 //System.out.println(" ==================   " + e.getMessage() + "   " + e.toString());
00540             }
00541         }
00542         else if(currentActUnit.getActionType().equals("PutOnTray")) {
00543             // look for the object at the pose 
00544             
00545             // update its relationship with the Robot, and remove its pose information
00546             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00547             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00548                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00549                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00550             String queryString = "SELECT DISTINCT ?tray WHERE { "
00551                 + "<" + mapNameSpace + OntoQueryUtil.RobotName + ">"
00552                 + " srs:hasPart ?tray . " 
00553                 + " ?tray a srs:COBTray . " 
00554                 + "}";
00555 
00556             try {
00557                 String targetObj = SpatialCalculator.nearestObject(this.recentDetectedObject, OntoQueryUtil.GlobalNameSpace + this.targetContent);
00558                 // System.out.println("TARGET OBJECT IS ::: " + targetObj);
00559                 if(!targetObj.trim().equals("")) {
00560                    
00561                     Pose tmpPose = new Pose();
00562                     tmpPose.position.x = -1000;
00563                     tmpPose.position.y = -1000;
00564                     tmpPose.position.z = -1000;
00565                     tmpPose.orientation.x = -1000;
00566                     tmpPose.orientation.y = -1000;
00567                     tmpPose.orientation.z = -1000;
00568                     tmpPose.orientation.w = -1000;
00569                     
00570                     // update its pose
00571                     try{
00572                         OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, targetObj.trim());
00573                         OntoQueryUtil.computeOnSpatialRelation();
00574                     }
00575                     catch(Exception e) {
00576                         //System.out.println(e.getMessage());
00577                     }
00578                 }
00579                
00580                 
00581                 // OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, OntoQueryUtil.ObjectNameSpace, this.targetContent);
00582                 ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix + queryString);
00583                 if(rset.size() == 0) {
00584                     //System.out.println("<<<< NO TRAY INSTANCE FOUND >>>");   
00585                 }
00586                 else {
00587                     //System.out.println("<<<< FOUND TRAY INSTANCE >>>");
00588                     Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(targetObj);
00589                     
00590                     OntoQueryUtil.removeAllSubPropertiesOf(targetObj, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00591                     QuerySolution qs = rset.get(0);
00592 
00593                     String gripper = qs.get("tray").toString();
00594                     //System.out.println("<<<<<  " + gripper + "  >>>>>");
00595 
00596                     Individual gripInd = KnowledgeEngine.ontoDB.getIndividual(gripper);
00597                     Property proExist = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00598                     
00599                     Property pro = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "aboveOf");
00600 
00601                     targetInd.setPropertyValue(pro, gripInd);
00602                 }
00603             }
00604             catch (Exception e) {
00605                 System.out.println(" ==================   " + e.getMessage() + " ----  " + e.toString());
00606             }
00607             
00608         }
00609     }
00610 
00611     private void updateTargetOfFailedAct() {
00612         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00613         HighLevelActionUnit currentActUnit = currentHLActSeq.getCurrentHighLevelActionUnit();
00614         if(currentActUnit.getActionType().equals("MoveAndDetection")) {
00615             //this.recentDetectedObject = ActionFeedback.toPose(fb);     
00616                     
00617             // update the knowledge (post-processing)
00618             String currentWorkSpace = (String)currentHLActSeq.getParameter("workspaceURI");
00619             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00620             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00621                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00622                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00623             String queryString = "SELECT DISTINCT ?obj WHERE { \n"
00624                 + "?obj srs:spatiallyRelated <" + currentWorkSpace + "> . \n" 
00625                 + " ?obj a srs:" + this.targetContent + " . \n" 
00626                 + "}";
00627             //          + " <" + currentWorkSpace + "> a srs:FurniturePiece . \n"
00628 
00629             ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix + queryString);
00630             if(rset.size() == 0) {
00631                 //System.out.println("<<<< NO OBJECT INSTANCE FOUND >>>");   
00632             }
00633             else {
00634                 //System.out.println("<<<< FOUND OBJECT INSTANCES ON WORKSPACE " + this.targetContent + " >>>");
00635                 //Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(target);
00636                 //Iterator targetIt = KnowledgeEngine.ontoDB.getInstancesOfClass(OntoQueryUtil.GlobalNameSpace + this.targetContent);
00637                 for (QuerySolution qs : rset) {
00638                     String temp = qs.get("obj").toString();
00639                     //System.out.println("<<<<<  " + temp + "  >>>>>");
00640                     
00641                     //System.out.println( temp.getNameSpace() + "   " + temp.getLocalName());
00642                     OntoQueryUtil.removeAllSubPropertiesOf(temp, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00643                     Pose tmpPose = new Pose();
00644                     tmpPose.position.x = -1000;
00645                     tmpPose.position.y = -1000;
00646                     tmpPose.position.z = -1000;
00647                     tmpPose.orientation.x = -1000;
00648                     tmpPose.orientation.y = -1000;
00649                     tmpPose.orientation.z = -1000;
00650                     tmpPose.orientation.w = -1000;
00651                     
00652                     // update its pose
00653                     try{
00654                         OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, temp.trim());
00655                         OntoQueryUtil.computeOnSpatialRelation();
00656                     }
00657                     catch(Exception e) {
00658                         //System.out.println(e.getMessage());
00659                     }
00660 
00661                 }
00662             }       
00663         }
00664         else if(currentActUnit.getActionType().equals("MoveAndGrasp")) {
00665             // do nothing
00666             
00667         }
00668         else if(currentActUnit.getActionType().equals("PutOnTray")) {
00669             // do nothing
00670             
00671         }
00672 
00673     }
00674 
00675     private CUAction handleSuccessMessageNew(ActionFeedback fb) {
00676         // TODO: 
00677         updateTargetOfSucceededActNew(fb);
00678     
00679         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00680 
00681         if(currentHLActSeq.hasNextHighLevelActionUnit()) {
00682             HighLevelActionUnit nextHighActUnit = currentHLActSeq.getNextHighLevelActionUnit();
00683             // set feedback? 
00684             if(nextHighActUnit.getActionType().equals("MoveAndGrasp") && !nextHighActUnit.ifParametersSet()) {
00685                 System.out.println("Next Action type is MoveAndGrasp"); 
00686                 Pose2D posBase = calculateGraspPosFromFBNew(fb);
00687                 if(posBase == null) {
00688                     System.out.println("No Grasping Post returned from Sym Grounding");
00689                     return handleFailedMessage();
00690                 }
00691 
00692                 String jsonMove = SRSJSONParser.encodeMoveAction("move", posBase.x, posBase.y, posBase.theta);
00693                 System.out.println("Grounding Returned Grasping Pose is (JSON): " + jsonMove);
00694 
00695                 if(!nextHighActUnit.setParameters("move", jsonMove, "")) {
00696                     //currentSubAction++;
00697                     return handleFailedMessage();               
00698                 }
00699 
00700             }
00701 
00702             if(nextHighActUnit != null) {
00703                 int tempI = nextHighActUnit.getNextCUActionIndex(true); 
00704                 // TODO: COULD BE DONE RECURSIVELY. BUT TOO COMPLEX UNNECESSARY AND DIFFICULT TO DEBUG. 
00705                 // SO STUPID CODE HERE
00706                 
00707                 if(tempI == HighLevelActionUnit.COMPLETED_SUCCESS) {
00708                     CUAction ca = new CUAction();
00709                     ca.status = 1;
00710                     return ca;
00711                 }
00712                 else if(tempI == HighLevelActionUnit.COMPLETED_FAIL || tempI == HighLevelActionUnit.INVALID_INDEX) {
00713                     CUAction ca = new CUAction();
00714                     ca.status = -1;
00715                     return ca;
00716                 }               
00717                 else {
00718                     CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00719                     // maybe actionUnit is empty
00720                     return ca == null ? handleFailedMessage() : ca;
00721                     /*
00722                     if (ca == null)  {
00723                         return handleFailedMessage();
00724                     }
00725 
00726                     return nextHighActUnit.getCUActionAt(tempI);
00727                     */
00728                 }
00729             }
00730         }
00731         return null;
00732     }
00733 
00734     private boolean updateDBObjectPose() {
00735         
00736         return true;
00737     }
00738 
00739     private Pose2D calculateGraspPosFromFBNew(ActionFeedback fb) {
00740         //calculateGraspPosition(SRSFurnitureGeometry furnitureInfo, Pose targetPose)
00741         // call symbol grounding to get parameters for the MoveAndGrasp action
00742         try {
00743             ros.pkg.srs_msgs.msg.SRSSpatialInfo furGeo = getFurnitureGeometryOf(workspaces.get(currentSubAction));
00744             //ros.pkg.srs_symbolic_grounding.msg.SRSSpatialInfo furGeo = newGetFurnitureGeometryOf(workspaces.get(currentSubAction));
00745             // TODO: recentDetectedObject should be updated accordingly when the MoveAndDetection action finished successfully
00746             //recentDetectedObject = ActionFeedback.toPose(fb);
00747             recentDetectedObject = fb.getDetectedObjectPose();
00748             if(recentDetectedObject == null) {
00749                 return null;
00750             }
00751 
00752             Pose2D pos = calculateGraspPosition(furGeo, recentDetectedObject);
00753             //Pose2D pos = newCalculateGraspPosition(furGeo, recentDetectedObject);
00754             return pos;
00755         }
00756         catch (Exception e) {
00757             System.out.println(e.getMessage() + " ++ " + e.toString());
00758             return null;
00759         }
00760         
00761     }
00762     
00763     private ArrayList<Pose2D> calculateScanPositions(ros.pkg.srs_msgs.msg.SRSSpatialInfo furnitureInfo) throws RosException {
00764         ArrayList<Pose2D> posList = new ArrayList<Pose2D>();
00765         ServiceClient<SymbolGroundingScanBasePose.Request, SymbolGroundingScanBasePose.Response, SymbolGroundingScanBasePose> sc =
00766             KnowledgeEngine.nodeHandle.serviceClient("symbol_grounding_scan_base_pose" , new SymbolGroundingScanBasePose(), false);
00767         
00768         SymbolGroundingScanBasePose.Request rq = new SymbolGroundingScanBasePose.Request();
00769         rq.parent_obj_geometry = furnitureInfo;
00770 
00771         SymbolGroundingScanBasePose.Response res = sc.call(rq);
00772         posList = res.scan_base_pose_list;
00773         sc.shutdown();
00774         return posList;
00775     }
00776     
00777     private Pose2D calculateGraspPosition(ros.pkg.srs_msgs.msg.SRSSpatialInfo  furnitureInfo, Pose targetPose) throws RosException {
00778         Pose2D pos = new Pose2D();
00779         
00780         ServiceClient<SymbolGroundingGraspBasePoseExperimental.Request, SymbolGroundingGraspBasePoseExperimental.Response, SymbolGroundingGraspBasePoseExperimental> sc = KnowledgeEngine.nodeHandle.serviceClient("symbol_grounding_grasp_base_pose_experimental" , new SymbolGroundingGraspBasePoseExperimental(), false);
00781         
00782         SymbolGroundingGraspBasePoseExperimental.Request rq = new SymbolGroundingGraspBasePoseExperimental.Request();
00783         rq.parent_obj_geometry = furnitureInfo;
00784         rq.target_obj_pose = targetPose;
00785 
00786         SymbolGroundingGraspBasePoseExperimental.Response res = sc.call(rq);
00787         boolean obstacleCheck = res.obstacle_check;
00788         double reach = res.reach;
00789         pos = res.grasp_base_pose;
00790 
00791         sc.shutdown();
00792         if(obstacleCheck) {
00793             return null;
00794         }
00795         return pos;
00796     }
00797         
00798     private Pose2D newCalculateGraspPosition(ros.pkg.srs_msgs.msg.SRSSpatialInfo furnitureInfo, Pose targetPose) throws RosException {
00799         //Pose2D pos = new Pose2D();
00800         //ArrayList<Pose2D> poses = new ArrayList<Pose2D>();
00801         Pose2D pose = new Pose2D();
00802         ServiceClient<SymbolGroundingGraspBasePose.Request, SymbolGroundingGraspBasePose.Response, SymbolGroundingGraspBasePose> sc = KnowledgeEngine.nodeHandle.serviceClient("symbol_grounding_grasp_base_pose" , new SymbolGroundingGraspBasePose(), false);
00803         
00804         SymbolGroundingGraspBasePose.Request rq = new SymbolGroundingGraspBasePose.Request();
00805         rq.parent_obj_geometry = furnitureInfo;
00806         rq.target_obj_pose = targetPose;
00807 
00808         SymbolGroundingGraspBasePose.Response res = sc.call(rq);
00809         boolean obstacleCheck = res.obstacle_check;
00810         double reach = res.reach;
00811         pose = res.grasp_base_pose;
00812 
00813         sc.shutdown();
00814         if(obstacleCheck) {
00815             return null;
00816         }
00817         return pose;
00818     }
00819     
00820     private ros.pkg.srs_msgs.msg.SRSSpatialInfo getFurnitureGeometryOf(Individual workspace) {
00821         ros.pkg.srs_msgs.msg.SRSSpatialInfo spatialInfo = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00822         //SRSFurnitureGeometry spatialInfo = new SRSFurnitureGeometry();
00823         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00824         spatialInfo.pose.position.x = stm.getFloat();
00825         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00826         spatialInfo.pose.position.y = stm.getFloat();
00827         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00828         spatialInfo.pose.position.z = stm.getFloat();
00829         
00830         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00831         spatialInfo.w = stm.getFloat();
00832         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00833         spatialInfo.h = stm.getFloat();
00834         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00835         spatialInfo.l = stm.getFloat();
00836         
00837         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00838         spatialInfo.pose.orientation.w = stm.getFloat();
00839         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00840         spatialInfo.pose.orientation.x = stm.getFloat();
00841         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00842         spatialInfo.pose.orientation.y = stm.getFloat();
00843         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00844         spatialInfo.pose.orientation.z = stm.getFloat();
00845         return spatialInfo;
00846     }
00847 
00848     private ros.pkg.srs_msgs.msg.SRSSpatialInfo newGetFurnitureGeometryOf(Individual workspace) {
00849         ros.pkg.srs_msgs.msg.SRSSpatialInfo spatialInfo = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00850         //SRSFurnitureGeometry spatialInfo = new SRSFurnitureGeometry();
00851         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00852         spatialInfo.pose.position.x = stm.getFloat();
00853         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00854         spatialInfo.pose.position.y = stm.getFloat();
00855         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00856         spatialInfo.pose.position.z = stm.getFloat();
00857         
00858         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00859         spatialInfo.w = stm.getFloat();
00860         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00861         spatialInfo.h = stm.getFloat();
00862         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00863         spatialInfo.l = stm.getFloat();
00864         
00865         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00866         spatialInfo.pose.orientation.w = stm.getFloat();
00867         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00868         spatialInfo.pose.orientation.x = stm.getFloat();
00869         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00870         spatialInfo.pose.orientation.y = stm.getFloat();
00871         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00872         spatialInfo.pose.orientation.z = stm.getFloat();
00873         return spatialInfo;
00874     }
00875 
00876 
00877     private boolean initTask(String targetContent) {
00878         //acts = new ArrayList<ActionTuple>();
00879         
00880         setTaskTarget(targetContent);
00881         System.out.println("TASK.JAVA: Created CurrentTask " + "get "
00882                            + targetContent);
00883         return constructTask();
00884     }   
00885 
00886     public boolean replan(OntologyDB onto, OntoQueryUtil ontoQuery) {
00887         return false;
00888     }
00889 
00890     public boolean isEmpty() {
00891         try {
00892             if(allSubSeqs.size() == 0) {
00893                 return true;
00894             }
00895         }
00896         catch(NullPointerException e) {
00897             System.out.println(e.getMessage() + "\n" + e.toString());
00898             return true;
00899         }
00900         return false;
00901     }
00902 
00903     private ArrayList<Individual> workspaces = new ArrayList<Individual>();
00904     private int currentSubAction;
00905     private Pose recentDetectedObject;    // required by MoveAndGraspActionUnit
00906     private String lastActionType;        // used to handle feedback from last action executed
00907     //private String userPose;
00908     private HighLevelActionUnit lastStepActUnit;
00909 }


srs_knowledge
Author(s): Ze Ji
autogenerated on Mon Oct 6 2014 08:27:32