CheckWorkspaceTask.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 ros.pkg.srs_knowledge.msg.*;
00057 import ros.pkg.geometry_msgs.msg.Pose2D;
00058 import ros.pkg.geometry_msgs.msg.Pose;
00059 import org.srs.srs_knowledge.knowledge_engine.*;
00060 import com.hp.hpl.jena.rdf.model.*;
00061 import com.hp.hpl.jena.query.QueryExecutionFactory;
00062 import com.hp.hpl.jena.query.ResultSet;
00063 import com.hp.hpl.jena.query.QueryExecution;
00064 import com.hp.hpl.jena.query.QuerySolution;
00065 import com.hp.hpl.jena.ontology.Individual;
00066 import org.srs.srs_knowledge.task.*;
00067 
00068 import ros.pkg.srs_symbolic_grounding.srv.*;
00069 import ros.pkg.srs_symbolic_grounding.msg.*;
00070 import ros.pkg.srs_msgs.msg.SRSSpatialInfo;
00071 
00072 import ros.*;
00073 import ros.communication.*;
00074 import org.json.simple.JSONArray;
00075 import org.json.simple.JSONObject;
00076 import org.json.simple.JSONValue;
00077 import org.json.simple.parser.ParseException;
00078 import org.json.simple.parser.JSONParser;
00079 
00080 public class CheckWorkspaceTask extends org.srs.srs_knowledge.task.Task
00081 {
00082     public CheckWorkspaceTask(String targetContent)
00083     {   
00084         this.initTask(targetContent);
00085     }
00086     private void initTask(String targetContent) {
00087         acts = new ArrayList<ActionTuple>();
00088         
00089         setTaskTarget(targetContent);
00090         System.out.println("TASK.JAVA: Created CurrentTask " + "get "
00091                            + targetContent);
00092         constructTask();
00093     }   
00094 
00095     protected boolean constructTask() {
00096         return createCheckWorkspaceTask();
00097     }
00098     
00099     private boolean createCheckWorkspaceTask() {
00100         System.out.println("Create New GET OBJECT Task --- ");
00101 
00102         try {
00103             workspaces = OntoQueryUtil.getWorkspaceByName(this.targetContent, OntoQueryUtil.ObjectNameSpace, OntoQueryUtil.GlobalNameSpace);
00104         }
00105         catch(Exception e) {
00106             System.out.println(e.getMessage() + "\n" + e.toString());
00107             return false;
00108         }
00109         
00110         for(Individual u : workspaces) {
00111             try{
00112                 System.out.println("Created HLActionSeq ");
00113                 HighLevelActionSequence subSeq = createSubSequenceForSingleWorkspace(u);
00114                 allSubSeqs.add(subSeq);
00115             }
00116             catch(RosException e) {
00117                 System.out.println("ROSEXCEPTION -- when calling symbolic grounding for scanning positions.  \n" + e.getMessage() + "\n" + e.toString());
00118                 
00119             }
00120             catch(Exception e) {
00121                 System.out.println(e.getMessage());
00122                 System.out.println(e.toString());
00123             }
00124         }       
00125         
00126         if(allSubSeqs.size() > 0) {
00127             currentSubAction = 0;
00128             return true;
00129         }
00130         else {
00131             currentSubAction = -1; // no sub_highlevel_action in list
00132             return false;
00133         }
00134     }
00135     
00136     private HighLevelActionSequence createSubSequenceForSingleWorkspace(Individual workspace) throws RosException, Exception {
00137         HighLevelActionSequence actionList = new HighLevelActionSequence();
00138         
00139         // create MoveAndDetectionActionUnit
00140         //SRSFurnitureGeometry spatialInfo = new SRSFurnitureGeometry();
00141         SRSSpatialInfo spatialInfo = new SRSSpatialInfo();
00142         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00143         
00144         spatialInfo.pose.position.x = stm.getFloat();
00145         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00146         spatialInfo.pose.position.y = stm.getFloat();
00147         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00148         spatialInfo.pose.position.z = stm.getFloat();
00149                 
00150         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00151         spatialInfo.w = stm.getFloat();
00152         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00153         spatialInfo.h = stm.getFloat();
00154         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00155         spatialInfo.l = stm.getFloat();
00156         
00157         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00158         spatialInfo.pose.orientation.w = stm.getFloat();
00159         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00160         spatialInfo.pose.orientation.x = stm.getFloat();
00161         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00162         spatialInfo.pose.orientation.y = stm.getFloat();
00163         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00164         spatialInfo.pose.orientation.z = stm.getFloat();
00165         
00166         // call symbolic grounding service for target pose
00167         ArrayList<Pose2D> posList;
00168         try {
00169             posList = calculateScanPositions(spatialInfo);
00170             System.out.println(posList.size());
00171         }
00172         catch(RosException e) {
00173             System.out.println(e.toString()); 
00174             System.out.println(e.getMessage());
00175             throw e;
00176         }
00177 
00178         // TODO:
00179         MoveAndCheckWorkspaceActionUnit mdAction = new MoveAndCheckWorkspaceActionUnit(posList, targetContent, spatialInfo);
00180         
00181         // create FinishActionUnit
00182         FinishActionUnit fau = new FinishActionUnit(true);
00183 
00184         actionList.appendHighLevelAction(mdAction);
00185         actionList.appendHighLevelAction(fau);
00186                 
00187         System.out.println("ActionList size is " + actionList.getSizeOfHighLevelActionList());
00188         return actionList;
00189     }
00190     
00191     @Override
00192     public CUAction getNextCUActionNew(boolean stateLastAction, String jsonFeedback) {
00193      
00194         System.out.println("===> Get Next CUACTION -- from CheckWorkspaceTask.java");
00195         CUAction ca = new CUAction();
00196         if(allSubSeqs.size() == 0 ) {
00197             System.out.println("Sequence size is zero");
00198             return null;   // ??? 
00199         }
00200         if(currentSubAction >= 0 && currentSubAction < allSubSeqs.size()) {
00201             // get the current SubActionSequence item
00202             System.out.println("Sequence size is " + allSubSeqs.size());
00203             HighLevelActionSequence subActSeq = allSubSeqs.get(currentSubAction);
00204             
00205             HighLevelActionUnit highAct = subActSeq.getCurrentHighLevelActionUnit();
00206             // decide if the current SubActionSequence is finished or stuck somewhere? 
00207             // if successfully finished, then finished
00208             // if stuck (fail), move to the next subActionSequence
00209             if(highAct != null) {
00210 
00211                 // TODO:
00212                 //updateDBObjectPose();
00213                                 
00214                 int ni = highAct.getNextCUActionIndex(stateLastAction);
00215                 System.out.println(" ========>>>>   " + ni);
00216                 switch(ni) {
00217                 case HighLevelActionUnit.COMPLETED_SUCCESS:
00218                     System.out.println(".COMPLETED_SUCCESS");
00219                     lastStepActUnit = highAct;
00220 
00221                     CUAction retact = null;                         
00222                     try {
00223                         retact = handleSuccessMessage(new ActionFeedback(jsonFeedback));
00224                     }
00225                     catch(ParseException pe) {
00226                         System.out.println(pe.toString());
00227                         return null;
00228                     }
00229  
00230                     return retact; 
00231                 case HighLevelActionUnit.COMPLETED_FAIL: 
00232                     lastStepActUnit = null;
00233                     System.out.println(".COMPLETED_FAIL");
00234                     return handleFailedMessage();
00235                 case HighLevelActionUnit.INVALID_INDEX:
00236                     lastStepActUnit = null;
00237                     System.out.println("INVALID_INDEX");
00238                     return handleFailedMessage();
00239                 default: 
00240                     System.out.println(highAct.getActionType());
00241                     if(!highAct.ifParametersSet()) {
00242                         System.out.println("Parameters not set");
00243                         lastStepActUnit = null;
00244                         return handleFailedMessage();
00245                     }
00246                     ca = highAct.getCUActionAt(ni);
00247                     // 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...
00248                     // WARNING: No error checking here
00249                     //lastActionType = ca.generic.actionInfo.get(0);
00250                     lastActionType = (String)(SRSJSONParser.decodeJsonActionInfo(ca.generic.jsonActionInfo).get("action"));
00251                     return ca;
00252                 } 
00253             }
00254             else {
00255                 return null;
00256             }       
00257             // or if still pending CUAction is available, return CUAction
00258         }
00259         else if (currentSubAction == -1) {
00260         }
00261         return ca;
00262     }
00263 
00264     private CUAction handleFailedMessage() {
00265         currentSubAction++;
00266         
00267         System.out.println("HANDLE FAILED MESSAGE.... CURRENTSUBACTION IS AT:  " + currentSubAction);
00268         
00269         if(currentSubAction >= allSubSeqs.size()) {
00270             return null;
00271         }
00272         HighLevelActionSequence nextHLActSeq = allSubSeqs.get(currentSubAction);
00273                 
00274         //      if(nextHLActSeq.hasNextHighLevelActionUnit()) {
00275         HighLevelActionUnit nextHighActUnit = nextHLActSeq.getCurrentHighLevelActionUnit();
00276         if(nextHighActUnit != null) {
00277             int tempI = nextHighActUnit.getNextCUActionIndex(true); 
00278             // TODO: COULD BE DONE RECURSIVELY. BUT TOO COMPLEX UNNECESSARY AND DIFFICULT TO DEBUG. 
00279             // SO STUPID CODE HERE
00280             
00281             if(tempI == HighLevelActionUnit.COMPLETED_SUCCESS || tempI == HighLevelActionUnit.COMPLETED_FAIL || tempI == HighLevelActionUnit.INVALID_INDEX) {
00282                 CUAction ca = new CUAction();
00283                 ca.status = -1;
00284                 return ca;
00285             }
00286             else {
00287                 System.out.println("GET NEXT CU ACTION AT:  " + tempI);
00288                 CUAction ca = nextHighActUnit.getCUActionAt(tempI);
00289                 if(ca == null) {
00290                     System.out.println("CUACTION IS NULL.......");
00291                 }
00292                 return ca;
00293             }
00294         }
00295         
00296         return null;
00297     }
00298     
00299     private CUAction handleSuccessMessage(ActionFeedback fb) {
00300         // TODO: 
00301           
00302         HighLevelActionSequence currentHLActSeq = allSubSeqs.get(currentSubAction);
00303         
00304         if(currentHLActSeq.hasNextHighLevelActionUnit()) {
00305             HighLevelActionUnit nextHighActUnit = currentHLActSeq.getNextHighLevelActionUnit();
00306             // set feedback? 
00307             if(nextHighActUnit != null) {
00308                 int tempI = nextHighActUnit.getNextCUActionIndex(true); 
00309                 // TODO: COULD BE DONE RECURSIVELY. BUT TOO COMPLEX UNNECESSARY AND DIFFICULT TO DEBUG. 
00310                 // SO STUPID CODE HERE
00311                 
00312                 if(tempI == HighLevelActionUnit.COMPLETED_SUCCESS) {
00313                     CUAction ca = new CUAction();
00314                     ca.status = 1;
00315                     return ca;
00316                 }
00317                 else if(tempI == HighLevelActionUnit.COMPLETED_FAIL || tempI == HighLevelActionUnit.INVALID_INDEX) {
00318                     CUAction ca = new CUAction();
00319                     ca.status = -1;
00320                     return ca;
00321                 }               
00322                 else {
00323                     return nextHighActUnit.getCUActionAt(tempI);
00324                 }
00325             }
00326         }
00327         
00328         return null;
00329     }
00330     
00331     private boolean updateDBObjectPose() {
00332         
00333         return true;
00334     }
00335     
00336     private ArrayList<Pose2D> calculateScanPositions(SRSSpatialInfo furnitureInfo) throws RosException {
00337         ArrayList<Pose2D> posList = new ArrayList<Pose2D>();
00338         ServiceClient<SymbolGroundingExploreBasePose.Request, SymbolGroundingExploreBasePose.Response, SymbolGroundingExploreBasePose> sc =
00339             KnowledgeEngine.nodeHandle.serviceClient("symbol_grounding_explore_base_pose" , new SymbolGroundingExploreBasePose(), false);
00340         
00341         SymbolGroundingExploreBasePose.Request rq = new SymbolGroundingExploreBasePose.Request();
00342         rq.parent_obj_geometry = furnitureInfo;
00343 
00344         SymbolGroundingExploreBasePose.Response res = sc.call(rq);
00345         posList = res.explore_base_pose_list;
00346         sc.shutdown();
00347         return posList;
00348     }
00349 
00350     private SRSSpatialInfo getFurnitureGeometryOf(Individual workspace) {
00351         SRSSpatialInfo spatialInfo = new SRSSpatialInfo();
00352         com.hp.hpl.jena.rdf.model.Statement stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord",  workspace);
00353         spatialInfo.pose.position.x = stm.getFloat();
00354         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord",  workspace);
00355         spatialInfo.pose.position.y = stm.getFloat();
00356         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord",  workspace);
00357         spatialInfo.pose.position.z = stm.getFloat();
00358         
00359         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject",  workspace);
00360         spatialInfo.w = stm.getFloat();
00361         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject",  workspace);
00362         spatialInfo.h = stm.getFloat();
00363         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject",  workspace);
00364         spatialInfo.l = stm.getFloat();
00365         
00366         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu",  workspace);
00367         spatialInfo.pose.orientation.w = stm.getFloat();
00368         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx",  workspace);
00369         spatialInfo.pose.orientation.x = stm.getFloat();
00370         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy",  workspace);
00371         spatialInfo.pose.orientation.y = stm.getFloat();
00372         stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz",  workspace);
00373         spatialInfo.pose.orientation.z = stm.getFloat();
00374         return spatialInfo;
00375     }
00376 
00377     @Override
00378     public boolean replan(OntologyDB onto, OntoQueryUtil ontoQuery) {
00379         return false;
00380     }
00381 
00382     @Override
00383     public boolean isEmpty() {
00384         try {
00385             if(allSubSeqs.size() == 0) {
00386                 return true;
00387             }
00388         }
00389         catch(NullPointerException e) {
00390             System.out.println(e.getMessage() + "\n" + e.toString());
00391             return true;
00392         }
00393         return false;
00394     }
00395 
00396     private ArrayList<Individual> workspaces = new ArrayList<Individual>();
00397     private int currentSubAction;
00398     private Pose recentDetectedObject;    // required by MoveAndGraspActionUnit
00399     private String lastActionType;        // used to handle feedback from last action executed
00400     private String userPose;
00401     private HighLevelActionUnit lastStepActUnit;
00402 }


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