$search
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 }