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


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