SearchObjectTask.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 import ros.pkg.srs_msgs.msg.SRSSpatialInfo;
00083 
00084 public class SearchObjectTask extends org.srs.srs_knowledge.task.Task
00085 {
00086     //private GetObjectTask.GraspType graspType = GetObjectTask.GraspType.MOVE_AND_GRASP;
00087     public SearchObjectTask(String targetContent) 
00088     {   
00089         //this.nodeHandle = n;
00090         // this.userPose = userPose;
00091         // this.init(taskType, targetContent, userPose);
00092         //this.initTask(targetContent);
00093         
00094         if (this.initTask(targetContent)) {
00095             System.out.println("Search Object Task constructed successfully -- " + targetContent);
00096         }
00097         else {
00098             System.out.println("Task not constructed successfully");
00099         }
00100     }
00101 
00102     /*
00103     public SearchObjectTask(String targetContent, GetObjectTask.GraspType graspMode) 
00104     {   
00105         this.graspType = graspMode;
00106         //this.nodeHandle = n;
00107         // this.userPose = userPose;
00108         // this.init(taskType, targetContent, userPose);
00109         this.initTask(targetContent);
00110     }
00111     */
00112 
00113     @Override
00114     protected boolean constructTask() {
00115         return createSearchObjectTask();
00116     }
00117     
00118     private boolean createSearchObjectTask() {
00119         /*
00120         // query for tables
00121         // move to tables (near -- use grounding)
00122         // detect milk
00123         */
00124         System.out.println("Create New GET OBJECT Task --- ");
00125 
00126         try {
00127             workspaces = OntoQueryUtil.getWorkspaceOfObject(this.targetContent, OntoQueryUtil.ObjectNameSpace, OntoQueryUtil.GlobalNameSpace, KnowledgeEngine.ontoDB);
00128         }
00129         catch(Exception e) {
00130             System.out.println(e.getMessage() + "\n" + e.toString());
00131             return false;
00132         }
00133         
00134         for(Individual u : workspaces) {
00135             System.out.println(u.getLocalName());
00136             try{
00137                 //System.out.println("Created HLActionSeq ");
00138                 HighLevelActionSequence subSeq = createSubSequenceForSingleWorkspace(u);
00139                 allSubSeqs.add(subSeq);
00140                
00141             }
00142             catch(RosException e) {
00143                 System.out.println("ROSEXCEPTION -- when calling symbolic grounding for scanning positions.  \t" + e.getMessage() + "\t" + e.toString());
00144                 
00145             }
00146             catch(Exception e) {
00147                 System.out.println(e.getMessage());
00148                 System.out.println(e.toString());
00149             }
00150         }       
00151         
00152         if(allSubSeqs.size() > 0) {
00153             currentSubAction = 0;
00154             return true;
00155         }
00156         else {
00157             currentSubAction = -1; // no sub_highlevel_action in list
00158             return false;
00159         }
00160     }
00161 
00162     
00163     private HighLevelActionSequence createSubSequenceForSingleWorkspace(Individual workspace) throws RosException, Exception {
00164         HighLevelActionSequence actionList = new HighLevelActionSequence();
00165         
00166         // create MoveAndDetectionActionUnit
00167         //SRSFurnitureGeometry spatialInfo = new SRSFurnitureGeometry();
00168         ros.pkg.srs_msgs.msg.SRSSpatialInfo spatialInfo = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00169         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00170         
00171         spatialInfo.pose.position.x = stm.getFloat();
00172         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00173         spatialInfo.pose.position.y = stm.getFloat();
00174         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00175         spatialInfo.pose.position.z = stm.getFloat();
00176                 
00177         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00178         spatialInfo.w = stm.getFloat();
00179         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00180         spatialInfo.h = stm.getFloat();
00181         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00182         spatialInfo.l = stm.getFloat();
00183         
00184         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00185         spatialInfo.pose.orientation.w = stm.getFloat();
00186         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00187         spatialInfo.pose.orientation.x = stm.getFloat();
00188         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00189         spatialInfo.pose.orientation.y = stm.getFloat();
00190         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00191         spatialInfo.pose.orientation.z = stm.getFloat();
00192         
00193         // call symbolic grounding service for target pose
00194         ArrayList<Pose2D> posList;
00195         try {
00196             posList = calculateScanPositions(spatialInfo);
00197             //System.out.println(posList.size());
00198         }
00199         catch(RosException e) {
00200             System.out.println(e.toString()); 
00201             System.out.println(e.getMessage());
00202             throw e;
00203         }
00204 
00205         Iterator<Individual> itInd = KnowledgeEngine.ontoDB.getInstancesOfClass(OntoQueryUtil.GlobalNameSpace + this.targetContent);
00206         int hhid = -1000;
00207         if(itInd.hasNext()) { 
00208             stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "houseHoldObjectID",  (Individual)itInd.next());
00209             hhid = OntoQueryUtil.getIntOfStatement(stm);
00210         }
00211 
00212         // TODO:
00213         MoveAndDetectionActionUnit mdAction = new MoveAndDetectionActionUnit(posList, targetContent, hhid, workspace.asResource().getLocalName());
00214         
00215         // create MoveAndGraspActionUnit
00216         //MoveAndGraspActionUnit mgAction = new MoveAndGraspActionUnit(null, targetContent, hhid, "side", workspace.asResource().getLocalName());
00217 
00218         // create PutOnTrayActionUnit
00219         //PutOnTrayActionUnit trayAction = new PutOnTrayActionUnit("side");
00220 
00221         // FoldArmActionUnit
00222         //FoldArmActionUnit foldArmAction = new FoldArmActionUnit();
00223 
00224         // create BackToUserActionUnit
00225         
00226         //Pose2D posUser = OntoQueryUtil.parsePose2D(userPose);
00227         //MoveActionUnit mau = new MoveActionUnit(posUser);
00228         
00229         // create FinishActionUnit
00230         FinishActionUnit fau = new FinishActionUnit(true);
00231         //MoveActionUnit mau1 = new MoveActionUnit(OntoQueryUtil.parsePose2D("home"));
00232         //actionList.appendHighLevelAction(mau1);
00233 
00234         actionList.appendHighLevelAction(mdAction);
00235         //actionList.appendHighLevelAction(mgAction);
00236         //actionList.appendHighLevelAction(trayAction);
00237         //actionList.appendHighLevelAction(foldArmAction);
00238         //actionList.appendHighLevelAction(mau);
00239         actionList.appendHighLevelAction(fau);
00240                 
00241         //System.out.println("ActionList size is " + actionList.getSizeOfHighLevelActionList());
00242 
00243         // set parameters for the workspace information
00244         actionList.setParameter("workspaceURI", workspace.asResource().getURI());
00245         actionList.setParameter("workspace", workspace.asResource().getLocalName());
00246         actionList.setParameter("workspaceNameSpace", workspace.asResource().getNameSpace());
00247 
00248         return actionList;
00249     }
00250     
00251     private CUAction handleFailedMessage() {
00252         // 
00253         updateTargetOfFailedAct();
00254 
00255         currentSubAction++;
00256         
00257         //System.out.println("HANDLE FAILED MESSAGE.... CURRENTSUBACTION IS AT:  " + currentSubAction);
00258         
00259         if(currentSubAction >= allSubSeqs.size()) {
00260             return null;
00261         }
00262         HighLevelActionSequence nextHLActSeq = allSubSeqs.get(currentSubAction);
00263                 
00264         //      if(nextHLActSeq.hasNextHighLevelActionUnit()) {
00265         HighLevelActionUnit nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00266         if(nextHighActUnit != null) {
00267             int tempI = nextHighActUnit.getNextCUActionIndex(true); 
00268 
00269             CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00270             
00271             while (ca == null) {
00272                 
00273                 System.out.println("CUACTION IS NULL....... Keep trying   " + currentSubAction);
00274                 currentSubAction++;
00275                 if(currentSubAction >= allSubSeqs.size()) {
00276                     return null;
00277                 }
00278                 nextHLActSeq = allSubSeqs.get(currentSubAction);
00279                 nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00280                 
00281                 tempI = nextHighActUnit.getNextCUActionIndex(true);
00282                 ca = nextHighActUnit.getCUActionAt(tempI); 
00283             }
00284  
00285             // TODO: COULD BE DONE RECURSIVELY. BUT TOO COMPLEX UNNECESSARY AND DIFFICULT TO DEBUG. 
00286             // SO STUPID CODE HERE
00287             /*
00288             if(tempI == HighLevelActionUnit.COMPLETED_SUCCESS || tempI == HighLevelActionUnit.COMPLETED_FAIL || tempI == HighLevelActionUnit.INVALID_INDEX) {
00289                 CUAction ca = new CUAction();
00290                 ca.status = -1;
00291                 return ca;
00292             }
00293             else {
00294                 //System.out.println("GET NEXT CU ACTION AT:  " + tempI);
00295                 CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00296 
00297                 while (ca == null) {
00298 
00299 
00300                     System.out.println("CUACTION IS NULL....... Keep trying   " + currentSubAction);
00301 
00302 
00303                     currentSubAction++;
00304         
00305         
00306                     if(currentSubAction >= allSubSeqs.size()) {
00307                         return null;
00308                     }
00309                     nextHLActSeq = allSubSeqs.get(currentSubAction);
00310                 
00311                     nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00312 
00313                     tempI = nextHighActUnit.getNextCUActionIndex(true);
00314                     ca = nextHighActUnit.getCUActionAt(tempI); 
00315                 }
00316             */
00317             /*
00318                 if(ca == null) {
00319                     System.out.println("CUACTION IS NULL.......");
00320                 }
00321                 */
00322             return ca;
00323             //}
00324         }
00325         
00326         return null;
00327     }
00328     
00329     private boolean updateDBObjectPose() {
00330         
00331         return true;
00332     }
00333     
00334     private ArrayList<Pose2D> calculateScanPositions(ros.pkg.srs_msgs.msg.SRSSpatialInfo furnitureInfo) throws RosException {
00335         ArrayList<Pose2D> posList = new ArrayList<Pose2D>();
00336         ServiceClient<SymbolGroundingScanBasePose.Request, SymbolGroundingScanBasePose.Response, SymbolGroundingScanBasePose> sc =
00337             KnowledgeEngine.nodeHandle.serviceClient("symbol_grounding_scan_base_pose" , new SymbolGroundingScanBasePose(), false);
00338         
00339         SymbolGroundingScanBasePose.Request rq = new SymbolGroundingScanBasePose.Request();
00340         rq.parent_obj_geometry = furnitureInfo;
00341 
00342         SymbolGroundingScanBasePose.Response res = sc.call(rq);
00343         posList = res.scan_base_pose_list;
00344         sc.shutdown();
00345         return posList;
00346     }
00347 
00348     private Pose2D calculateGraspPosition(ros.pkg.srs_msgs.msg.SRSSpatialInfo furnitureInfo, Pose targetPose) throws RosException {
00349         Pose2D pos = new Pose2D();
00350         
00351         ServiceClient<SymbolGroundingGraspBasePoseExperimental.Request, SymbolGroundingGraspBasePoseExperimental.Response, SymbolGroundingGraspBasePoseExperimental> sc = KnowledgeEngine.nodeHandle.serviceClient("symbol_grounding_grasp_base_pose_experimental" , new SymbolGroundingGraspBasePoseExperimental(), false);
00352         
00353         SymbolGroundingGraspBasePoseExperimental.Request rq = new SymbolGroundingGraspBasePoseExperimental.Request();
00354         rq.parent_obj_geometry = furnitureInfo;
00355         rq.target_obj_pose = targetPose;
00356 
00357         SymbolGroundingGraspBasePoseExperimental.Response res = sc.call(rq);
00358         boolean obstacleCheck = res.obstacle_check;
00359         double reach = res.reach;
00360         pos = res.grasp_base_pose;
00361 
00362         sc.shutdown();
00363         if(obstacleCheck) {
00364             return null;
00365         }
00366         return pos;
00367     }
00368 
00369     private ros.pkg.srs_msgs.msg.SRSSpatialInfo getFurnitureGeometryOf(Individual workspace) {
00370         //SRSFurnitureGeometry spatialInfo = new SRSFurnitureGeometry();
00371         ros.pkg.srs_msgs.msg.SRSSpatialInfo spatialInfo = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00372         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00373         spatialInfo.pose.position.x = stm.getFloat();
00374         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00375         spatialInfo.pose.position.y = stm.getFloat();
00376         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00377         spatialInfo.pose.position.z = stm.getFloat();
00378         
00379         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00380         spatialInfo.w = stm.getFloat();
00381         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00382         spatialInfo.h = stm.getFloat();
00383         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00384         spatialInfo.l = stm.getFloat();
00385         
00386         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00387         spatialInfo.pose.orientation.w = stm.getFloat();
00388         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00389         spatialInfo.pose.orientation.x = stm.getFloat();
00390         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00391         spatialInfo.pose.orientation.y = stm.getFloat();
00392         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00393         spatialInfo.pose.orientation.z = stm.getFloat();
00394         return spatialInfo;
00395     }
00396 
00397     private boolean initTask(String targetContent) {
00398         acts = new ArrayList<ActionTuple>();
00399         
00400         setTaskTarget(targetContent);
00401         System.out.println("TASK.JAVA: Created CurrentTask " + "get "
00402                            + targetContent);
00403         return constructTask();
00404     }   
00405 
00406     @Override
00407     public boolean replan(OntologyDB onto, OntoQueryUtil ontoQuery) {
00408         return false;
00409     }
00410 
00411     @Override
00412     public boolean isEmpty() {
00413         try {
00414             if(allSubSeqs.size() == 0) {
00415                 return true;
00416             }
00417         }
00418         catch(NullPointerException e) {
00419             System.out.println(e.getMessage() + "\n" + e.toString());
00420             return true;
00421         }
00422         return false;
00423     }
00424 
00425     private void updateTargetOfFailedAct() {
00426         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00427         HighLevelActionUnit currentActUnit = currentHLActSeq.getCurrentHighLevelActionUnit();
00428         if(currentActUnit.getActionType().equals("MoveAndDetection")) {
00429             //this.recentDetectedObject = ActionFeedback.toPose(fb);     
00430                     
00431             // update the knowledge (post-processing)
00432             String currentWorkSpace = (String)currentHLActSeq.getParameter("workspaceURI");
00433             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00434             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00435                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00436                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00437             String queryString = "SELECT DISTINCT ?obj WHERE { \n"
00438                 + "?obj srs:spatiallyRelated <" + currentWorkSpace + "> . \n" 
00439                 + " ?obj a srs:" + this.targetContent + " . \n" 
00440                 + "}";
00441             //          + " <" + currentWorkSpace + "> a srs:FurniturePiece . \n"
00442 
00443             ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix + queryString);
00444             if(rset.size() == 0) {
00445                 System.out.println("<<<< NO OBJECT INSTANCE FOUND >>>");   
00446             }
00447             else {
00448                 //System.out.println("<<<< FOUND OBJECT INSTANCES ON WORKSPACE " + this.targetContent + " >>>");
00449                 //Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(target);
00450                 //Iterator targetIt = KnowledgeEngine.ontoDB.getInstancesOfClass(OntoQueryUtil.GlobalNameSpace + this.targetContent);
00451                 for (QuerySolution qs : rset) {
00452                     String temp = qs.get("obj").toString();
00453                     //System.out.println("<<<<<  " + temp + "  >>>>>");
00454                     
00455                     //System.out.println( temp.getNameSpace() + "   " + temp.getLocalName());
00456                     OntoQueryUtil.removeAllSubPropertiesOf(temp, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00457                     Pose tmpPose = new Pose();
00458                     tmpPose.position.x = -1000;
00459                     tmpPose.position.y = -1000;
00460                     tmpPose.position.z = -1000;
00461                     tmpPose.orientation.x = -1000;
00462                     tmpPose.orientation.y = -1000;
00463                     tmpPose.orientation.z = -1000;
00464                     tmpPose.orientation.w = -1000;
00465                     
00466                     // update its pose
00467                     try{
00468                         OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, temp.trim());
00469                         OntoQueryUtil.computeOnSpatialRelation();
00470                     }
00471                     catch(Exception e) {
00472                         System.out.println(e.getMessage());
00473                     }
00474 
00475                 }
00476             }       
00477         }
00478         else if(currentActUnit.getActionType().equals("MoveAndGrasp")) {
00479             // do nothing
00480         }
00481         else if(currentActUnit.getActionType().equals("PutOnTray")) {
00482             // do nothing
00483         }
00484     }
00485 
00489     // TODO:   NOT COMPLETED... 
00490     @Override
00491     public CUAction getNextCUActionNew(boolean stateLastAction, String jsonFeedback) {
00492         System.out.println("===> Get Next CUACTION -- from GetObjectTask.java");
00493         CUAction ca = new CUAction();
00494         if(allSubSeqs.size() == 0 ) {
00495             System.out.println("Sequence size is zero");
00496             return null;   // ??? 
00497         }
00498         if(currentSubAction >= 0 && currentSubAction < allSubSeqs.size()) {
00499             // get the current SubActionSequence item
00500             System.out.println("Sequence size is " + allSubSeqs.size());
00501             HighLevelActionSequence subActSeq = allSubSeqs.get(currentSubAction);
00502             
00503             HighLevelActionUnit highAct = subActSeq.getCurrentHighLevelActionUnit();
00504             // decide if the current SubActionSequence is finished or stuck somewhere? 
00505             // if successfully finished, then finished
00506             // if stuck (fail), move to the next subActionSequence
00507             if(highAct != null) {
00508 
00509                 // TODO:
00510                 updateDBObjectPose();
00511                                 
00512                 int ni = highAct.getNextCUActionIndex(stateLastAction); 
00513                 switch(ni) {
00514                 case HighLevelActionUnit.COMPLETED_SUCCESS:
00515                     System.out.println("COMPLETED_SUCCESS");
00516                     lastStepActUnit = highAct;
00517 
00518                     CUAction retact = null;
00519                     try {
00520                         retact = handleSuccessMessageNew(new ActionFeedback(jsonFeedback));
00521                     }
00522                     catch(ParseException pe) {
00523                         System.out.println(pe.toString());
00524                         return null;
00525                     }
00526 
00527                     return retact; 
00528                 case HighLevelActionUnit.COMPLETED_FAIL:
00529                     // The whole task finished (failure). 
00530                     lastStepActUnit = null;
00531                     System.out.println("COMPLETED_FAIL");
00532                     return handleFailedMessage();
00533                 case HighLevelActionUnit.INVALID_INDEX:
00534                     // The whole task finished failure. Should move to a HighLevelActionUnit in subActSeq of finsihing
00535                     lastStepActUnit = null;
00536                     System.out.println("INVALID_INDEX");
00537                     //currentSubAction++;
00538                     return handleFailedMessage();
00539                 default: 
00540                     System.out.println(highAct.getActionType());
00541                     if(!highAct.ifParametersSet()) {
00542                         System.out.println("Parameters not set");
00543                         lastStepActUnit = null;
00544                         return handleFailedMessage();
00545                     }
00546                     ca = highAct.getCUActionAt(ni);
00547 
00548                     // maybe actionUnit is empty
00549                     if (ca == null)  {
00550                         return handleFailedMessage();
00551                     }
00552 
00553                     // 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...
00554                     // WARNING: No error checking here
00555                     //lastActionType = ca.generic.actionInfo.get(0);
00556                     lastActionType = (String)(SRSJSONParser.decodeJsonActionInfo(ca.generic.jsonActionInfo).get("action"));
00557 
00558                     return ca;
00559                 } 
00560             }
00561             else {
00562                 return null;
00563             }       
00564             // or if still pending CUAction is available, return CUAction
00565         }
00566         else if (currentSubAction == -1) {
00567         }
00568 
00569         return ca;
00570     }
00571 
00575     private void updateTargetOfSucceededActNew(ActionFeedback fb) {
00576         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00577         HighLevelActionUnit currentActUnit = currentHLActSeq.getCurrentHighLevelActionUnit();
00578         if(currentActUnit.getActionType().equals("MoveAndDetection")) {
00579             //this.recentDetectedObject = ActionFeedback.toPose(fb);
00580             this.recentDetectedObject = fb.getDetectedObjectPose();
00581             BoundingBoxDim bbDim = InformationRetrieval.retrieveBoundingBoxInfo(OntoQueryUtil.GlobalNameSpace + this.targetContent);
00582             ros.pkg.srs_msgs.msg.SRSSpatialInfo spaObj = new ros.pkg.srs_msgs.msg.SRSSpatialInfo();
00583             spaObj.l = bbDim.l;
00584             spaObj.h = bbDim.h;
00585             spaObj.w = bbDim.w;
00586 
00587             spaObj.pose = this.recentDetectedObject;
00588 
00589             // update the knowledge (post-processing)
00590             
00591             // if there exists one same object on the same workspace, update it --- simple solution
00592             //String ws = SpatialCalculator.workspaceHolding(spaObj);
00593             
00594             String neighbour = SpatialCalculator.nearestObject(spaObj.pose, OntoQueryUtil.GlobalNameSpace + this.targetContent);
00595             if(!neighbour.trim().equals("")) {
00596                 //System.out.println("Found neighbour of " + neighbour);
00597                 // update its pose
00598                 try{
00599                     OntoQueryUtil.updatePoseOfObject(spaObj.pose, OntoQueryUtil.GlobalNameSpace, neighbour.trim());
00600                     OntoQueryUtil.computeOnSpatialRelation();
00601                 }
00602                 catch(Exception e) {
00603                     System.out.println(e.getMessage());
00604                 }
00605             }
00606             
00607             // if there does not exist such an object, then insert a new one
00608             // bounding box can be obtained from HHDB
00609             // TODO
00610             
00611         }
00612         else if(currentActUnit.getActionType().equals("MoveAndGrasp")) {
00613             // look for the object at the pose 
00614             // update its relationship with the Robot, and remove its pose information
00615             //      Individual rob = KnowledgeEngine.ontoDB.getIndividual(OntoQueryUtil.ObjectNameSpace + OntoQueryUtil.RobotName);
00616             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00617             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00618                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00619                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00620             String queryString = "SELECT DISTINCT ?gripper WHERE { "
00621                 + "<" + mapNameSpace + OntoQueryUtil.RobotName + ">"
00622                 + " srs:hasPart ?gripper . " 
00623                 + " ?gripper a srs:RobotGripper . " 
00624                 + "}";
00625 
00626             try {
00627                 String targetObj = SpatialCalculator.nearestObject(this.recentDetectedObject, OntoQueryUtil.GlobalNameSpace + this.targetContent);
00628                 //System.out.println("TARGET OBJECT IS ::: " + targetObj);
00629                 if(!targetObj.trim().equals("")) {
00630                    
00631                     Pose tmpPose = new Pose();
00632                     tmpPose.position.x = -1000;
00633                     tmpPose.position.y = -1000;
00634                     tmpPose.position.z = -1000;
00635                     tmpPose.orientation.x = -1000;
00636                     tmpPose.orientation.y = -1000;
00637                     tmpPose.orientation.z = -1000;
00638                     tmpPose.orientation.w = -1000;
00639                     
00640                     // update its pose
00641                     try{
00642                         OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, targetObj.trim());
00643                         OntoQueryUtil.computeOnSpatialRelation();
00644                     }
00645                     catch(Exception e) {
00646                         System.out.println(e.getMessage());
00647                     }
00648                 }
00649                
00650                 
00651                 // OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, OntoQueryUtil.ObjectNameSpace, this.targetContent);
00652                 ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix + queryString);
00653                 if(rset.size() == 0) {
00654                     //System.out.println("<<<< NO GRIPPER INSTANCE FOUND >>>");   
00655                 }
00656                 else {
00657                     //System.out.println("<<<< FOUND GRIPPER INSTANCE >>>");
00658                     Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(targetObj);
00659                     
00660                     OntoQueryUtil.removeAllSubPropertiesOf(targetObj, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00661                     QuerySolution qs = rset.get(0);
00662 
00663                     String gripper = qs.get("gripper").toString();
00664                     //System.out.println("<<<<<  " + gripper + "  >>>>>");
00665 
00666                     Individual gripInd = KnowledgeEngine.ontoDB.getIndividual(gripper);
00667                     Property proExist = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00668                     
00669                     Property pro = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "grippedBy");
00670 
00671                     targetInd.setPropertyValue(pro, gripInd);
00672                 }
00673             }
00674             catch (Exception e) {
00675                 //System.out.println(" ==================   " + e.getMessage() + "   " + e.toString());
00676             }
00677         }
00678         else if(currentActUnit.getActionType().equals("PutOnTray")) {
00679             // look for the object at the pose 
00680             
00681             // update its relationship with the Robot, and remove its pose information
00682             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00683             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00684                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00685                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00686             String queryString = "SELECT DISTINCT ?tray WHERE { "
00687                 + "<" + mapNameSpace + OntoQueryUtil.RobotName + ">"
00688                 + " srs:hasPart ?tray . " 
00689                 + " ?tray a srs:COBTray . " 
00690                 + "}";
00691 
00692             try {
00693                 String targetObj = SpatialCalculator.nearestObject(this.recentDetectedObject, OntoQueryUtil.GlobalNameSpace + this.targetContent);
00694                 // System.out.println("TARGET OBJECT IS ::: " + targetObj);
00695                 if(!targetObj.trim().equals("")) {
00696                    
00697                     Pose tmpPose = new Pose();
00698                     tmpPose.position.x = -1000;
00699                     tmpPose.position.y = -1000;
00700                     tmpPose.position.z = -1000;
00701                     tmpPose.orientation.x = -1000;
00702                     tmpPose.orientation.y = -1000;
00703                     tmpPose.orientation.z = -1000;
00704                     tmpPose.orientation.w = -1000;
00705                     
00706                     // update its pose
00707                     try{
00708                         OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, targetObj.trim());
00709                         OntoQueryUtil.computeOnSpatialRelation();
00710                     }
00711                     catch(Exception e) {
00712                         //System.out.println(e.getMessage());
00713                     }
00714                 }
00715                
00716                 
00717                 // OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, OntoQueryUtil.ObjectNameSpace, this.targetContent);
00718                 ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix + queryString);
00719                 if(rset.size() == 0) {
00720                     //System.out.println("<<<< NO TRAY INSTANCE FOUND >>>");   
00721                 }
00722                 else {
00723                     //System.out.println("<<<< FOUND TRAY INSTANCE >>>");
00724                     Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(targetObj);
00725                     
00726                     OntoQueryUtil.removeAllSubPropertiesOf(targetObj, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00727                     QuerySolution qs = rset.get(0);
00728 
00729                     String gripper = qs.get("tray").toString();
00730                     //System.out.println("<<<<<  " + gripper + "  >>>>>");
00731 
00732                     Individual gripInd = KnowledgeEngine.ontoDB.getIndividual(gripper);
00733                     Property proExist = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "spatiallyRelated");
00734                     
00735                     Property pro = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "aboveOf");
00736 
00737                     targetInd.setPropertyValue(pro, gripInd);
00738                 }
00739             }
00740             catch (Exception e) {
00741                 System.out.println(" ==================   " + e.getMessage() + " ----  " + e.toString());
00742             }
00743             
00744         }
00745     }
00746 
00747     private CUAction handleSuccessMessageNew(ActionFeedback fb) {
00748         // TODO: 
00749         updateTargetOfSucceededActNew(fb);
00750     
00751         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00752 
00753         if(currentHLActSeq.hasNextHighLevelActionUnit()) {
00754             HighLevelActionUnit nextHighActUnit = currentHLActSeq.getNextHighLevelActionUnit();
00755             // set feedback? 
00756             if(nextHighActUnit.getActionType().equals("MoveAndGrasp") && !nextHighActUnit.ifParametersSet()) {
00757 
00758                 Pose2D posBase = calculateGraspPosFromFBNew(fb);
00759                 if(posBase == null) {
00760                     return handleFailedMessage();
00761                 }
00762 
00763                 String jsonMove = SRSJSONParser.encodeMoveAction("move", posBase.x, posBase.y, posBase.theta);
00764                 if(!nextHighActUnit.setParameters("move", jsonMove, "")) {
00765                     //currentSubAction++;
00766                     return handleFailedMessage();               
00767                 }
00768                 /*
00769                 ArrayList<String> basePos = constructArrayFromPose2D(posBase);
00770                 if(!nextHighActUnit.setParameters(basePos)) {
00771                     //currentSubAction++;
00772                     return handleFailedMessage();               
00773                 }
00774                 */
00775             }
00776 
00777             if(nextHighActUnit != null) {
00778                 int tempI = nextHighActUnit.getNextCUActionIndex(true); 
00779                 // TODO: COULD BE DONE RECURSIVELY. BUT TOO COMPLEX UNNECESSARY AND DIFFICULT TO DEBUG. 
00780                 // SO STUPID CODE HERE
00781                 
00782                 if(tempI == HighLevelActionUnit.COMPLETED_SUCCESS) {
00783                     CUAction ca = new CUAction();
00784                     ca.status = 1;
00785                     return ca;
00786                 }
00787                 else if(tempI == HighLevelActionUnit.COMPLETED_FAIL || tempI == HighLevelActionUnit.INVALID_INDEX) {
00788                     CUAction ca = new CUAction();
00789                     ca.status = -1;
00790                     return ca;
00791                 }               
00792                 else {
00793                     CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00794                     // maybe actionUnit is empty
00795                     return ca == null ? handleFailedMessage() : ca;
00796                     
00797                     //return nextHighActUnit.getCUActionAt(tempI);
00798                 }
00799             }
00800         }
00801         return null;
00802     }
00803 
00804     private Pose2D calculateGraspPosFromFBNew(ActionFeedback fb) {
00805         //calculateGraspPosition(SRSFurnitureGeometry furnitureInfo, Pose targetPose)
00806         // call symbol grounding to get parameters for the MoveAndGrasp action
00807         try {
00808             //SRSFurnitureGeometry furGeo = getFurnitureGeometryOf(workspaces.get(currentSubAction));
00809             ros.pkg.srs_msgs.msg.SRSSpatialInfo furGeo = getFurnitureGeometryOf(workspaces.get(currentSubAction));
00810             //ros.pkg.srs_symbolic_grounding.msg.SRSSpatialInfo furGeo = newGetFurnitureGeometryOf(workspaces.get(currentSubAction));
00811             // TODO: recentDetectedObject should be updated accordingly when the MoveAndDetection action finished successfully
00812             //recentDetectedObject = ActionFeedback.toPose(fb);
00813             recentDetectedObject = fb.getDetectedObjectPose();
00814             if(recentDetectedObject == null) {
00815                 return null;
00816             }
00817 
00818             Pose2D pos = calculateGraspPosition(furGeo, recentDetectedObject);
00819             //Pose2D pos = newCalculateGraspPosition(furGeo, recentDetectedObject);
00820             return pos;
00821         }
00822         catch (Exception e) {
00823             System.out.println(e.getMessage() + " ++ " + e.toString());
00824             return null;
00825         }
00826         
00827     }
00828 
00829 
00830     private ArrayList<Individual> workspaces = new ArrayList<Individual>();
00831     private int currentSubAction;
00832     private Pose recentDetectedObject;    // required by MoveAndGraspActionUnit
00833     private String lastActionType;        // used to handle feedback from last action executed
00834     //private String userPose;
00835     private HighLevelActionUnit lastStepActUnit;
00836 }


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