$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.knowledge_engine; 00052 00053 import com.hp.hpl.jena.rdf.model.*; 00054 import com.hp.hpl.jena.vocabulary.*; 00055 import com.hp.hpl.jena.util.FileManager; 00056 import com.hp.hpl.jena.ontology.OntClass; 00057 import com.hp.hpl.jena.ontology.SomeValuesFromRestriction; 00058 import com.hp.hpl.jena.ontology.Restriction; 00059 import com.hp.hpl.jena.query.Query; 00060 import com.hp.hpl.jena.query.QueryFactory; 00061 import com.hp.hpl.jena.query.ResultSetFormatter; 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 com.hp.hpl.jena.ontology.OntProperty; 00068 import java.io.*; 00069 import java.util.ArrayList; 00070 import java.util.ConcurrentModificationException; 00071 import java.util.NoSuchElementException; 00072 import java.util.HashSet; 00073 import java.util.Iterator; 00074 import java.util.Collections; 00075 import java.util.Comparator; 00076 import ros.*; 00077 import ros.communication.*; 00078 import ros.pkg.srs_knowledge.srv.QuerySparQL; 00079 import ros.pkg.srs_knowledge.msg.*; 00080 //import ros.pkg.srs_knowledge.msg.SRSSpatialInfo; 00081 import ros.pkg.srs_msgs.msg.SRSSpatialInfo; 00082 import com.hp.hpl.jena.shared.Lock; 00083 import ros.pkg.srs_knowledge.srv.PlanNextAction; 00084 import ros.pkg.srs_knowledge.srv.TaskRequest; 00085 import ros.pkg.srs_knowledge.srv.GetObjectsOnMap; 00086 import ros.pkg.srs_knowledge.srv.GetWorkspaceOnMap; 00087 import com.hp.hpl.jena.rdf.model.Statement; 00088 import org.srs.srs_knowledge.task.*; 00089 import ros.pkg.geometry_msgs.msg.Pose2D; 00090 import ros.pkg.geometry_msgs.msg.Pose; 00091 import ros.pkg.srs_knowledge.srv.GetObjectsOnMap; 00092 import ros.pkg.srs_knowledge.srv.GetWorkspaceOnMap; 00093 00094 import java.util.Properties; 00095 00096 import java.io.IOException; 00097 import java.io.*; 00098 import java.util.StringTokenizer; 00099 import java.util.ArrayList; 00100 import java.util.HashMap; 00101 import java.util.Iterator; 00102 import java.util.Map; 00103 import java.util.Set; 00104 00105 import org.srs.srs_knowledge.utils.*; 00106 00111 public class OntoQueryUtil 00112 { 00113 public static ArrayList<Individual> getWorkspaceOfObject(String objectClassName, String objectNameSpace, String globalNameSpace, OntologyDB onto) { 00114 // TODO: 00115 ArrayList<String> workspaceList = getWorkspaceNamesOfObject(objectClassName, objectNameSpace, globalNameSpace); 00116 ArrayList<Individual> workspaceIndList = new ArrayList<Individual>(); 00117 for(String s : workspaceList) { 00118 workspaceIndList.add(onto.getModel().getIndividual(objectNameSpace + s)); 00119 } 00120 return workspaceIndList; 00121 } 00122 00123 public static ArrayList<String> getWorkspaceNamesOfObject(String objectClassName) { 00124 return OntoQueryUtil.getWorkspaceNamesOfObject(objectClassName, OntoQueryUtil.ObjectNameSpace, OntoQueryUtil.GlobalNameSpace); 00125 } 00126 00130 public static ArrayList<String> getWorkspaceNamesOfObject(String objectClassName, String objectNameSpace, String globalNameSpace) { 00131 ArrayList<String> workspaceList = new ArrayList<String>(); 00132 00133 String className = globalNameSpace + objectClassName; 00134 00135 // first, retrieve instance(s) of objectClassName 00136 Iterator<Individual> instancesOfObject = KnowledgeEngine.ontoDB.getInstancesOfClass(className); 00137 00138 // second, retrieve instance(s) of workspace for this particular objectClassName 00139 com.hp.hpl.jena.rdf.model.Statement stm; 00140 00141 for( ; instancesOfObject.hasNext(); ) { 00142 stm = KnowledgeEngine.ontoDB.getPropertyOf(globalNameSpace, "spatiallyRelated", instancesOfObject.next()); 00143 if (stm != null) { 00144 workspaceList.add(stm.getObject().asResource().getLocalName()); 00145 } 00146 } 00147 // list possible workspace(s), e.g. tables 00148 ArrayList<String> otherWorkspaces = getFurnituresLinkedToObject(objectClassName); 00149 00150 Iterator<Individual> otherInstances; 00151 00152 for(int i = 0; i < otherWorkspaces.size(); i++) { 00153 otherInstances = KnowledgeEngine.ontoDB.getInstancesOfClass(globalNameSpace + otherWorkspaces.get(i)); 00154 workspaceList = OntoQueryUtil.addUniqueInstances(workspaceList, otherInstances); 00155 //otherWSInd = OntoQueryUtil.addUniqueInstances(otherWSInd, otherInstances); 00156 } 00157 00158 /* 00159 // To sort by distance... TODO: robot current location is unknown... to implement in future 00160 Collections.sort(otherWSInd, new Comparator<String>() { 00161 public int compare(String ind1, String ind2) { 00162 00163 // TODO;;; 00164 Pose p1 = OntoQueryUtil.getPoseOfObject(ind1); 00165 Pose p2 = OntoQueryUtil.getPoseOfObject(ind2); 00166 return 0; 00167 } 00168 } 00169 ); 00170 workspaceList.addAll(otherWSInd); 00171 */ 00172 00173 return workspaceList; 00174 } 00175 00176 public static Pose getPoseOfObject(String objectURI) { 00177 00178 Pose p = new Pose(); 00179 00180 com.hp.hpl.jena.rdf.model.Statement stm; 00181 try{ 00182 Individual temp = KnowledgeEngine.ontoDB.getIndividual(objectURI); 00183 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord", temp); 00184 p.position.x = getFloatOfStatement(stm); 00185 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord", temp); 00186 p.position.y = getFloatOfStatement(stm); 00187 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord", temp); 00188 p.position.z = getFloatOfStatement(stm); 00189 00190 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu", temp); 00191 p.orientation.w = getFloatOfStatement(stm); 00192 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx", temp); 00193 p.orientation.x = getFloatOfStatement(stm); 00194 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy", temp); 00195 p.orientation.y = getFloatOfStatement(stm); 00196 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz", temp); 00197 p.orientation.z = getFloatOfStatement(stm); 00198 } 00199 catch(NonExistenceEntryException e) { 00200 return null; 00201 } 00202 catch(Exception e) { 00203 //System.out.println("CAUGHT exception: " + e.getMessage()+ ".. added invalid values"); 00204 00205 p.position.x = -1000; 00206 p.position.y = -1000; 00207 p.position.z = -1000; 00208 00209 p.orientation.w = -1000; 00210 p.orientation.x = -1000; 00211 p.orientation.y = -1000; 00212 p.orientation.z = -1000; 00213 } 00214 00215 return p; 00216 } 00217 00218 public static ArrayList<String> addUniqueInstances(ArrayList<String> original, Iterator<Individual> newList) { 00219 while(newList.hasNext()) { 00220 Individual tempIndividual = newList.next(); 00221 String temp = tempIndividual.getLocalName(); 00222 if (!original.contains(temp)) { 00223 original.add(temp); 00224 } 00225 } 00226 00227 return original; 00228 } 00229 00230 public static ArrayList<String> tempGetFurnituresLinkedToObject(String objectClassName) { 00231 // TODO: Only temporarily provided for object related furnitures or workspaces . should be obtained from ontology instead 00232 00233 // temporarily create the data 00234 Map<String, ArrayList<String>> mpWorkspaces = new HashMap<String, ArrayList<String>>(); 00235 ArrayList<String> mb = new ArrayList<String>(); 00236 mb.add("Table-PieceOfFurniture"); 00237 mb.add("Dishwasher"); 00238 mpWorkspaces.put("Milkbox", mb); 00239 mpWorkspaces.put("Salt", mb); 00240 mpWorkspaces.put("Bottle", mb); 00241 mpWorkspaces.put("Pringles", mb); 00242 mpWorkspaces.put("Medicine", mb); 00243 00244 ArrayList<String> mbook = new ArrayList<String>(); 00245 mbook.add("Table-PieceOfFurniture"); 00246 mbook.add("Dishwasher"); 00247 mbook.add("BookShelf"); 00248 mpWorkspaces.put("Book", mbook); 00249 mpWorkspaces.put("BookCopy", mbook); 00250 00251 return mpWorkspaces.get(objectClassName); 00252 } 00253 00254 private static HashSet<String> getSuperAndSubClassesOf(String classURI, String rootClassURI, boolean includeSelf) { 00255 HashSet<String> ret = new HashSet<String>(); 00256 String nsDef = 00257 "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n" 00258 + " PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n" 00259 + " PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>\n"; 00260 String queryString = " SELECT ?sup WHERE {\n" 00261 + "<" + classURI + "> rdfs:subClassOf ?sup . \n" 00262 + " ?sup rdfs:subClassOf <" + rootClassURI + "> .}\n"; 00263 ArrayList<QuerySolution> qr = KnowledgeEngine.ontoDB.executeQueryRaw(nsDef + queryString); 00264 for(int i = 0; i < qr.size(); i++) { 00265 String tmp = qr.get(i).getResource("sup").getURI(); 00266 ret.add(tmp); 00267 } 00268 if(includeSelf) { 00269 ret.add(classURI); 00270 } 00271 00272 return ret; 00273 } 00274 00275 //return local names 00276 public static ArrayList<String> getFurnituresLinkedToObject(String objectClassName) { 00277 ArrayList<String> ret = new ArrayList<String>(); 00278 OntClass objClass = KnowledgeEngine.ontoDB.model.getOntClass(OntoQueryUtil.GlobalNameSpace + objectClassName ); 00279 /* 00280 for(Iterator<OntProperty> pros = objClass.listDeclaredProperties(true); pros.hasNext(); ) { 00281 System.out.println(pros.next().getLocalName()); 00282 } 00283 */ 00284 HashSet<String> supClasses = OntoQueryUtil.getSuperAndSubClassesOf(OntoQueryUtil.GlobalNameSpace + objectClassName, OntoQueryUtil.GlobalNameSpace + "GraspableObject", true); 00285 Iterator<String> itSup = supClasses.iterator(); 00286 while(itSup.hasNext()) { 00287 objClass = KnowledgeEngine.ontoDB.model.getOntClass(itSup.next()); 00288 for (Iterator<OntClass> supers = objClass.listSuperClasses(false); supers.hasNext(); ) { 00289 OntClass sup = supers.next(); 00290 if (sup.isRestriction()) { 00291 if (sup.asRestriction().isSomeValuesFromRestriction()) { 00292 //displayRestriction( "some", sup.getOnProperty(), sup.asSomeValuesFromRestriction().getSomeValuesFrom() ); 00293 //} 00294 //displayRestriction( sup.asRestriction() ); 00295 OntProperty pro = sup.asRestriction().asSomeValuesFromRestriction().getOnProperty(); 00296 //System.out.println(" Found Pro --- " + pro.asResource().toString()); 00297 00298 if(pro.getURI().equals(OntoQueryUtil.GlobalNameSpace + "storedAtPlace")) { 00299 Resource objRes = sup.asRestriction().asSomeValuesFromRestriction().getSomeValuesFrom(); 00300 ret.add(objRes.getLocalName()); 00301 //System.out.println(); 00302 } 00303 } 00304 // displayType( supers.next() ); 00305 } 00306 } 00307 } 00308 00309 return ret; 00310 } 00311 00312 public static Pose2D parsePose2D(String targetContent) { 00313 Pose2D pos = new Pose2D(); 00314 00315 double x = 1; 00316 double y = 1; 00317 double theta = 0; 00318 00319 if (targetContent.charAt(0) == '[' 00320 && targetContent.charAt(targetContent.length() - 1) == ']') { 00321 StringTokenizer st = new StringTokenizer(targetContent, " [],"); 00322 if (st.countTokens() == 3) { 00323 try { 00324 x = Double.parseDouble(st.nextToken()); 00325 y = Double.parseDouble(st.nextToken()); 00326 theta = Double.parseDouble(st.nextToken()); 00327 System.out.println(x + " " + y + " " + theta); 00328 } catch (Exception e) { 00329 System.out.println(e.getMessage()); 00330 return null; 00331 } 00332 } 00333 } else { 00334 // Ontology queries 00335 /* 00336 String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n" 00337 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n" 00338 + "PREFIX ipa-kitchen-map: <http://www.srs-project.eu/ontologies/ipa-kitchen-map.owl#>\n"; 00339 String queryString = "SELECT ?x ?y ?theta WHERE { " 00340 + "ipa-kitchen-map:" + targetContent 00341 + " srs:xCoordinate ?x . " + "ipa-kitchen-map:" 00342 + targetContent + " srs:yCoordinate ?y . " 00343 + "ipa-kitchen-map:" + targetContent 00344 + " srs:orientationTheta ?theta .}"; 00345 */ 00346 String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n" 00347 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n" 00348 + "PREFIX map: <" + OntoQueryUtil.ObjectNameSpace + ">\n"; 00349 00350 String queryString = "SELECT ?x ?y ?theta WHERE { " 00351 + "map:" + targetContent 00352 + " srs:xCoordinate ?x . " 00353 + "map:" + targetContent + " srs:yCoordinate ?y . " 00354 + "map:" + targetContent + " srs:orientationTheta ?theta .}"; 00355 //System.out.println(prefix + queryString + "\n"); 00356 00357 if (KnowledgeEngine.ontoDB == null) { 00358 System.out.println("Ontology Database is NULL"); 00359 return null; 00360 } 00361 00362 try { 00363 ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix 00364 + queryString); 00365 if (rset.size() == 0) { 00366 System.out.println("ERROR: No move target found from database"); 00367 return null; 00368 } else if (rset.size() == 1) { 00369 System.out 00370 .println("INFO: OK info retrieved from DB... "); 00371 QuerySolution qs = rset.get(0); 00372 x = qs.getLiteral("x").getFloat(); 00373 y = qs.getLiteral("y").getFloat(); 00374 theta = qs.getLiteral("theta").getFloat(); 00375 System.out.println("x is " + x + ". y is " + y 00376 + ". theta is " + theta); 00377 } else { 00378 System.out.println("WARNING: Multiple options... "); 00379 QuerySolution qs = rset.get(0); 00380 x = qs.getLiteral("x").getFloat(); 00381 y = qs.getLiteral("y").getFloat(); 00382 theta = qs.getLiteral("theta").getFloat(); 00383 System.out.println("x is " + x + ". y is " + y 00384 + ". theta is " + theta); 00385 } 00386 } catch (Exception e) { 00387 System.out.println("Exception --> " + e.getMessage()); 00388 return null; 00389 } 00390 } 00391 00392 pos.x = x; 00393 pos.y = y; 00394 pos.theta = theta; 00395 return pos; 00396 } 00397 00398 public static ArrayList<Individual> getWorkspaceByName(String objectClassName, String objectNameSpace, String globalNameSpace) { 00399 // TODO: 00400 String className = globalNameSpace + objectClassName; 00401 00402 // first, retrieve instance(s) of objectClassName 00403 Iterator<Individual> workspaceIndList = KnowledgeEngine.ontoDB.getInstancesOfClass(className); 00404 ArrayList<Individual> wList = new ArrayList<Individual>(); 00405 for( ; workspaceIndList.hasNext(); ) { 00406 wList.add(workspaceIndList.next()); 00407 } 00408 return wList; 00409 } 00410 00411 public static boolean testUpdateObjectProperty(String proNSURI, String objectNSURI, String objectName) throws NonExistenceEntryException 00412 { 00413 try { 00414 Individual ind = KnowledgeEngine.ontoDB.getIndividual(objectNSURI + objectName); 00415 // set property 00416 Property pro = KnowledgeEngine.ontoDB.getProperty(proNSURI + "xCoord"); 00417 // ind.setPropertyValue(pro, ); 00418 com.hp.hpl.jena.rdf.model.Statement stm = ind.getProperty(pro); 00419 // KnowledgeEngine.ontoDB.removeStatement(stm); 00420 00421 Literal x = KnowledgeEngine.ontoDB.model.createTypedLiteral(10.0f); 00422 ind.setPropertyValue(pro, x); 00423 00424 //stm.changeLiteralObject(10.0f); 00425 00426 //Statement stm1 = KnowledgeEngine.ontoDB.model.createStatement(ind, pro, x); 00427 //KnowledgeEngine.ontoDB.model.add(stm1); 00428 //model.leaveCriticalSection(); 00429 } 00430 catch(NonExistenceEntryException e) { 00431 throw e; 00432 } 00433 return true; 00434 } 00435 00436 public static boolean updatePoseOfObject(Pose pos, String propertyNSURI, String objectURI) throws NonExistenceEntryException { 00437 System.out.println("Update the pose of an object or furniture in the semantic map"); 00438 try { 00439 Individual ind = KnowledgeEngine.ontoDB.getIndividual(objectURI); 00440 // set property 00441 Property pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "xCoord"); 00442 // com.hp.hpl.jena.rdf.model.Statement stm = ind.getProperty(pro); 00443 //Literal x = KnowledgeEngine.ontoDB.model.createTypedLiteral(10.0f); 00444 Literal x = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.position.x)); 00445 ind.setPropertyValue(pro, x); 00446 00447 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "yCoord"); 00448 Literal y = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.position.y)); 00449 ind.setPropertyValue(pro, y); 00450 00451 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "zCoord"); 00452 Literal z = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.position.z)); 00453 ind.setPropertyValue(pro, z); 00454 00455 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "qx"); 00456 Literal qx = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.orientation.x)); 00457 ind.setPropertyValue(pro, qx); 00458 00459 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "qy"); 00460 Literal qy = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.orientation.y)); 00461 ind.setPropertyValue(pro, qy); 00462 00463 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "qz"); 00464 Literal qz = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.orientation.z)); 00465 ind.setPropertyValue(pro, qz); 00466 00467 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "qu"); 00468 Literal qw = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.orientation.w)); 00469 ind.setPropertyValue(pro, qw); 00470 00471 // Update symbolic spatial relation 00472 OntoQueryUtil.computeOnSpatialRelation(); 00473 } 00474 catch(NonExistenceEntryException e) { 00475 throw e; 00476 } 00477 00478 return true; 00479 } 00480 00481 public static boolean updatePoseOfObject(Pose pos, String propertyNSURI, String objectNSURI, String objectName) throws NonExistenceEntryException { 00482 return updatePoseOfObject(pos, propertyNSURI, objectNSURI + objectName); 00483 } 00484 00485 public static boolean updateDimensionOfObject(float l, float w, float h, String propertyNSURI, String objectNSURI, String objectName) throws NonExistenceEntryException { 00486 System.out.println("Update the dimension of an object or furniture in the semantic map"); 00487 try{ 00488 Individual ind = KnowledgeEngine.ontoDB.getIndividual(objectNSURI + objectName); 00489 // set property 00490 Property pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "lengthOfObject"); 00491 com.hp.hpl.jena.rdf.model.Statement stm = ind.getProperty(pro); 00492 00493 Literal ll = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(l)); 00494 ind.setPropertyValue(pro, ll); 00495 00496 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "widthOfObject"); 00497 stm = ind.getProperty(pro); 00498 Literal lw = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(w)); 00499 ind.setPropertyValue(pro, lw); 00500 00501 pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "heightOfObject"); 00502 stm = ind.getProperty(pro); 00503 Literal lh = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(h)); 00504 ind.setPropertyValue(pro, lh); 00505 } 00506 catch(NonExistenceEntryException e) { 00507 throw e; 00508 } 00509 00510 return true; 00511 } 00512 00513 public static boolean updateHHIdOfObject(int id, String propertyNSURI, String objectNSURI, String objectName) throws NonExistenceEntryException { 00514 System.out.println("Update the household object ID of an object or furniture in the semantic map"); 00515 try{ 00516 Individual ind = KnowledgeEngine.ontoDB.getIndividual(objectNSURI + objectName); 00517 // set property 00518 Property pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "houseHoldObjectID"); 00519 com.hp.hpl.jena.rdf.model.Statement stm = ind.getProperty(pro); 00520 Literal litId = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Integer(id)); 00521 ind.setPropertyValue(pro, litId); 00522 } 00523 catch(NonExistenceEntryException e) { 00524 throw e; 00525 } 00526 return true; 00527 } 00528 00529 public static boolean computeOnSpatialRelation() { 00530 // get a list of objects and workspaces 00531 GetObjectsOnMap.Response resOBJ = getObjectsOnMap(OntoQueryUtil.MapName, true); 00532 GetWorkspaceOnMap.Response resWS = getWorkspaceOnMap(OntoQueryUtil.MapName, true); 00533 // pair-wise comparison --- if condition met, then update the knowledge base 00534 //System.out.println("RESULT ------->>>>>> SIZE OF OBJECTS: " + resOBJ.objects.size()); 00535 //for (String obj : resOBJ.objects) { 00536 // System.out.println(obj); 00537 //} 00538 00539 //System.out.println("RESULT ------>>>>>> SIZE OF WORKSPACES: " + resWS.objects.size()); 00540 //for (String ws : resWS.objects) { 00541 // System.out.println(ws); 00542 //} 00543 00544 for (int i = 0; i < resOBJ.objectsInfo.size(); i++) { 00545 for (int j = 0; j < resWS.objectsInfo.size(); j++) { 00546 if(SpatialCalculator.ifOnObject(resOBJ.objectsInfo.get(i), resWS.objectsInfo.get(j), -1)) { 00547 //System.out.println("FOUND ONE PAIR MATCH THE ON RELATIONSHIP"); 00548 //TODO update rdf graph model 00549 try { 00550 Individual ind1 = KnowledgeEngine.ontoDB.getIndividual(OntoQueryUtil.ObjectNameSpace + resOBJ.objects.get(i)); 00551 Individual ind2 = KnowledgeEngine.ontoDB.getIndividual(OntoQueryUtil.ObjectNameSpace + resWS.objects.get(j)); 00552 //Property proExist = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "spatiallyRelated"); 00553 //ind1.removeProperty(proExist, ind2); 00554 OntoQueryUtil.removeAllSubPropertiesOf(OntoQueryUtil.ObjectNameSpace + resOBJ.objects.get(i), OntoQueryUtil.GlobalNameSpace + "spatiallyRelated"); 00555 00556 if (OntoQueryUtil.updateOnSpatialRelation(ind1, ind2) ) { 00557 // System.out.println("Added Property: Object " + resOBJ.objects.get(i) + " is aboveOf Object " + resWS.objects.get(j)); 00558 } 00559 else { 00560 //System.out.println("CANNOT add Property: Object " + resOBJ.objects.get(i) + " is aboveOf Object " + resWS.objects.get(j)); 00561 } 00562 00563 } 00564 catch (NonExistenceEntryException e) { 00565 System.out.println("Individual not found in ontology: --- " + e.getMessage()); 00566 } 00567 00568 } 00569 else { 00570 //System.out.println("NO MATCH.... between : " + resOBJ.objects.get(i) + " and " + resWS.objects.get(j)); 00571 00572 } 00573 } 00574 } 00575 00576 // TODO: Better to use SPARQL CONSTRUCT + RULES ... TO REPLACE 00577 00578 return true; 00579 } 00580 00581 private static boolean updateOnSpatialRelation(Individual obj1, Individual obj2) { 00582 try { 00583 Property pro = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "aboveOf"); 00584 //com.hp.hpl.jena.rdf.model.Statement stm = obj1.getProperty(pro); 00585 obj1.setPropertyValue(pro, obj2); 00586 } 00587 catch(NonExistenceEntryException e) { 00588 System.out.println("NonExistenceEntryException --> " + e.getMessage()); 00589 return false; 00590 } 00591 /* 00592 Individual ind = KnowledgeEngine.ontoDB.getIndividual(objectNSURI + objectName); 00593 // set property 00594 Property pro = KnowledgeEngine.ontoDB.getProperty(propertyNSURI + "xCoord"); 00595 com.hp.hpl.jena.rdf.model.Statement stm = ind.getProperty(pro); 00596 //Literal x = KnowledgeEngine.ontoDB.model.createTypedLiteral(10.0f); 00597 Literal x = KnowledgeEngine.ontoDB.model.createTypedLiteral(new Float(pos.position.x)); 00598 ind.setPropertyValue(pro, x); 00599 */ 00600 return true; 00601 } 00602 00603 public static GetWorkspaceOnMap.Response getWorkspaceOnMap(String map, boolean ifGeometryInfo) { 00604 GetWorkspaceOnMap.Response re = new GetWorkspaceOnMap.Response(); 00605 String className = GlobalNameSpace; 00606 String mapNS = ObjectNameSpace; 00607 00608 if(map != null) { 00609 if(KnowledgeEngine.ontoDB.getNamespaceByPrefix(map) != null) { 00610 mapNS = KnowledgeEngine.ontoDB.getNamespaceByPrefix(map); 00611 } 00612 } 00613 00614 className = className + "FurniturePiece"; 00615 //System.out.println(className); 00616 try{ 00617 Iterator<Individual> instances = KnowledgeEngine.ontoDB.getInstancesOfClass(className); 00618 if(instances == null) { 00619 return re; 00620 } 00621 com.hp.hpl.jena.rdf.model.Statement stm; 00622 00623 //if(instances.hasNext()) { 00624 while (instances.hasNext()) { 00625 Individual temp = (Individual)instances.next(); 00626 //System.out.println( temp.getNameSpace() + " " + temp.getLocalName()); 00627 if(temp.getNameSpace().equals(ObjectNameSpace)) { 00628 re.objects.add(temp.getLocalName()); 00629 re.classesOfObjects.add(temp.getRDFType(true).getLocalName()); 00630 try{ 00631 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "houseHoldObjectID", temp); 00632 re.houseHoldId.add(Integer.toString(getIntOfStatement(stm))); 00633 } 00634 catch(Exception e) { 00635 //System.out.println("CAUGHT exception: " + e.toString()); 00636 re.houseHoldId.add("NA"); 00637 } 00638 00639 String readableName = temp.getLocalName(); 00640 try{ 00641 com.hp.hpl.jena.rdf.model.NodeIterator nit = KnowledgeEngine.ontoDB.listPropertiesOf(OntoQueryUtil.GlobalNameSpace, "objectReadableName", temp); 00642 //stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "objectReadableName", temp); 00643 while(nit.hasNext()) { 00644 RDFNode n = nit.next(); 00645 Literal l = n.asLiteral(); 00646 //System.out.println(l.getLanguage() + " " + l.getString()); 00647 if(l.getLanguage().equalsIgnoreCase(OntoQueryUtil.Language)) { 00648 readableName = l.getString(); 00649 break; 00650 } 00651 } 00652 } 00653 catch(Exception e) { 00654 System.out.println("CAUGHT exception: " + e.toString() + " Cannot retrieve readable name for object. Use Empty instead"); 00655 //re.readableNames.add(""); 00656 } 00657 finally { 00658 re.readableNames.add(readableName); 00659 } 00660 00661 // encode other properties in json for json_properties 00662 Map<String, String> pros = new HashMap<String, String>(); 00663 00664 try{ 00665 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "insideOf", temp); 00666 String r = stm.getObject().asResource().getLocalName(); 00667 pros.put("insideOf", r); 00668 } 00669 catch(Exception e) { 00670 System.out.println("CAUGHT exception: " + e.toString()); 00671 } 00672 re.json_properties.add(SRSJSONParser.encodeObjectProperties(pros)); 00673 00674 if(ifGeometryInfo == true) { 00675 SRSSpatialInfo spatialInfo = new SRSSpatialInfo(); 00676 00677 try{ 00678 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord", temp); 00679 spatialInfo.pose.position.x = getFloatOfStatement(stm); 00680 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord", temp); 00681 spatialInfo.pose.position.y = getFloatOfStatement(stm); 00682 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord", temp); 00683 spatialInfo.pose.position.z = getFloatOfStatement(stm); 00684 00685 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject", temp); 00686 spatialInfo.w = getFloatOfStatement(stm); 00687 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject", temp); 00688 spatialInfo.h = getFloatOfStatement(stm); 00689 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject", temp); 00690 spatialInfo.l = getFloatOfStatement(stm); 00691 00692 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu", temp); 00693 spatialInfo.pose.orientation.w = getFloatOfStatement(stm); 00694 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx", temp); 00695 spatialInfo.pose.orientation.x = getFloatOfStatement(stm); 00696 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy", temp); 00697 spatialInfo.pose.orientation.y = getFloatOfStatement(stm); 00698 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz", temp); 00699 spatialInfo.pose.orientation.z = getFloatOfStatement(stm); 00700 00701 } 00702 catch(Exception e) { 00703 //System.out.println("CAUGHT exception: " + e.getMessage()+ ".. added invalid values"); 00704 00705 spatialInfo.pose.position.x = -1000; 00706 spatialInfo.pose.position.y = -1000; 00707 spatialInfo.pose.position.z = -1000; 00708 00709 spatialInfo.w = -1000; 00710 spatialInfo.h = -1000; 00711 spatialInfo.l = -1000; 00712 00713 spatialInfo.pose.orientation.w = -1000; 00714 spatialInfo.pose.orientation.x = -1000; 00715 spatialInfo.pose.orientation.y = -1000; 00716 spatialInfo.pose.orientation.z = -1000; 00717 } 00718 00719 re.objectsInfo.add(spatialInfo); 00720 } 00721 } 00722 } 00723 } 00724 catch(Exception e) { 00725 System.out.println(e.getMessage()); 00726 } 00727 00728 return re; 00729 } 00730 00731 public static GetObjectsOnMap.Response getObjectsOnMapOfType(String objectTypeURI, boolean ifGeometryInfo) { 00732 GetObjectsOnMap.Response re = new GetObjectsOnMap.Response(); 00733 //System.out.println(objectTypeURI + " --- "); 00734 Iterator<Individual> instances = KnowledgeEngine.ontoDB.getInstancesOfClass(objectTypeURI); 00735 if(instances == null) { 00736 return re; 00737 } 00738 com.hp.hpl.jena.rdf.model.Statement stm; 00739 if(instances.hasNext()) { 00740 while (instances.hasNext()) { 00741 Individual temp = (Individual)instances.next(); 00742 //System.out.println( temp.getNameSpace() + " " + temp.getLocalName()); 00743 if(temp.getNameSpace().equals(ObjectNameSpace)) { 00744 re.objects.add(temp.getLocalName()); 00745 re.classesOfObjects.add(temp.getRDFType(true).getLocalName()); 00746 try{ 00747 stm = KnowledgeEngine.ontoDB.getPropertyOf(GlobalNameSpace, "spatiallyRelated", temp); 00748 //stm = KnowledgeEngine.ontoDB.getPropertyOf(GlobalNameSpace, "aboveOf", temp); 00749 re.spatialRelation.add(stm.getPredicate().getLocalName()); 00750 re.spatialRelatedObject.add(stm.getObject().asResource().getLocalName()); 00751 } 00752 catch(Exception e) { 00753 //System.out.println("CAUGHT exception: " + e.toString()); 00754 re.spatialRelation.add("NA"); 00755 re.spatialRelatedObject.add("NA"); 00756 } 00757 try{ 00758 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "houseHoldObjectID", temp); 00759 re.houseHoldId.add(Integer.toString(getIntOfStatement(stm))); 00760 } 00761 catch(Exception e) { 00762 //System.out.println("CAUGHT exception: " + e.toString()); 00763 re.houseHoldId.add("NA"); 00764 } 00765 00766 String readableName = temp.getLocalName(); 00767 try{ 00768 com.hp.hpl.jena.rdf.model.NodeIterator nit = KnowledgeEngine.ontoDB.listPropertiesOf(OntoQueryUtil.GlobalNameSpace, "objectReadableName", temp); 00769 //stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "objectReadableName", temp); 00770 while(nit.hasNext()) { 00771 RDFNode n = nit.next(); 00772 Literal l = n.asLiteral(); 00773 //System.out.println(l.getLanguage() + " " + l.getString()); 00774 if(l.getLanguage().equalsIgnoreCase(OntoQueryUtil.Language)) { 00775 readableName = l.getString(); 00776 break; 00777 } 00778 } 00779 } 00780 catch(Exception e) { 00781 System.out.println("CAUGHT exception: " + e.toString() + " Cannot retrieve readable name for object. Use Empty instead"); 00782 //re.readableNames.add(""); 00783 } 00784 finally { 00785 re.readableNames.add(readableName); 00786 } 00787 00788 if(ifGeometryInfo == true) { 00789 SRSSpatialInfo spatialInfo = new SRSSpatialInfo(); 00790 try{ 00791 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "xCoord", temp); 00792 spatialInfo.pose.position.x = getFloatOfStatement(stm); 00793 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "yCoord", temp); 00794 spatialInfo.pose.position.y = getFloatOfStatement(stm); 00795 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "zCoord", temp); 00796 spatialInfo.pose.position.z = getFloatOfStatement(stm); 00797 00798 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "widthOfObject", temp); 00799 spatialInfo.w = getFloatOfStatement(stm); 00800 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "heightOfObject", temp); 00801 spatialInfo.h = getFloatOfStatement(stm); 00802 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "lengthOfObject", temp); 00803 spatialInfo.l = getFloatOfStatement(stm); 00804 00805 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qu", temp); 00806 spatialInfo.pose.orientation.w = getFloatOfStatement(stm); 00807 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qx", temp); 00808 spatialInfo.pose.orientation.x = getFloatOfStatement(stm); 00809 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qy", temp); 00810 spatialInfo.pose.orientation.y = getFloatOfStatement(stm); 00811 stm = KnowledgeEngine.ontoDB.getPropertyOf(OntoQueryUtil.GlobalNameSpace, "qz", temp); 00812 spatialInfo.pose.orientation.z = getFloatOfStatement(stm); 00813 } 00814 catch(Exception e) { 00815 //System.out.println("CAUGHT exception: " + e.getMessage()+ ".. added invalid values"); 00816 00817 spatialInfo.pose.position.x = -1000; 00818 spatialInfo.pose.position.y = -1000; 00819 spatialInfo.pose.position.z = -1000; 00820 00821 spatialInfo.w = -1000; 00822 spatialInfo.h = -1000; 00823 spatialInfo.l = -1000; 00824 00825 spatialInfo.pose.orientation.w = -1000; 00826 spatialInfo.pose.orientation.x = -1000; 00827 spatialInfo.pose.orientation.y = -1000; 00828 spatialInfo.pose.orientation.z = -1000; 00829 } 00830 00831 re.objectsInfo.add(spatialInfo); 00832 } 00833 } 00834 } 00835 } 00836 else { 00837 System.out.println("<EMPTY>"); 00838 } 00839 00840 return re; 00841 } 00842 00843 public static GetObjectsOnMap.Response getObjectsOnMap(String map, boolean ifGeometryInfo) { 00844 //GetObjectsOnMap.Response re = new GetObjectsOnMap.Response(); 00845 00846 String className = GlobalNameSpace; 00847 String mapNS = ObjectNameSpace; 00848 00849 if(map != null) { 00850 if(KnowledgeEngine.ontoDB.getNamespaceByPrefix(map) != null) { 00851 mapNS = KnowledgeEngine.ontoDB.getNamespaceByPrefix(map); 00852 } 00853 } 00854 00855 // className = className + "FoodVessel"; 00856 className = className + "GraspableObject"; 00857 00858 return OntoQueryUtil.getObjectsOnMapOfType(className, ifGeometryInfo); 00859 } 00860 00861 public static void removeAllSubPropertiesOf(String subjectURI, String propertyURI) { 00862 try{ 00863 Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(subjectURI); 00864 OntProperty proExist = KnowledgeEngine.ontoDB.getOntProperty(propertyURI); 00865 00866 com.hp.hpl.jena.util.iterator.ExtendedIterator<OntProperty> subPros = (com.hp.hpl.jena.util.iterator.ExtendedIterator<OntProperty>)proExist.listSubProperties(); 00867 00868 while(subPros.hasNext()) { 00869 com.hp.hpl.jena.rdf.model.Statement stm = targetInd.getProperty(subPros.next()); 00870 if(stm != null) { 00871 KnowledgeEngine.ontoDB.removeStatement(stm); 00872 targetInd.removeAll(proExist); 00873 } 00874 } 00875 } 00876 catch(Exception e) { 00877 //System.out.println(e.toString() + " " + e.getMessage()); 00878 return; 00879 } 00880 } 00881 00882 public static void testRemoveProperty() { 00883 try { 00884 String targetObj = OntoQueryUtil.ObjectNameSpace + "MilkBox0"; 00885 System.out.println("TARGET OBJECT IS ::: " + targetObj); 00886 if(!targetObj.trim().equals("")) { 00887 Pose tmpPose = new Pose(); 00888 tmpPose.position.x = -1000; 00889 tmpPose.position.y = -1000; 00890 tmpPose.position.z = -1000; 00891 tmpPose.orientation.x = -1000; 00892 tmpPose.orientation.y = -1000; 00893 tmpPose.orientation.z = -1000; 00894 tmpPose.orientation.w = -1000; 00895 00896 // update its pose 00897 try{ 00898 OntoQueryUtil.updatePoseOfObject(tmpPose, OntoQueryUtil.GlobalNameSpace, targetObj.trim()); 00899 OntoQueryUtil.computeOnSpatialRelation(); 00900 } 00901 catch(Exception e) { 00902 System.out.println(e.getMessage() + " " + e.toString()); 00903 } 00904 } 00905 00906 Individual targetInd = KnowledgeEngine.ontoDB.getIndividual(targetObj); 00907 00908 OntoQueryUtil.removeAllSubPropertiesOf(targetObj, OntoQueryUtil.GlobalNameSpace + "spatiallyRelated"); 00909 00910 String gripper = OntoQueryUtil.ObjectNameSpace + "SRSCOBGripper"; 00911 //System.out.println("<<<<< " + gripper + " >>>>>"); 00912 00913 Individual gripInd = KnowledgeEngine.ontoDB.getIndividual(gripper); 00914 00915 /* 00916 OntProperty proExist = KnowledgeEngine.ontoDB.getOntProperty(OntoQueryUtil.GlobalNameSpace + "spatiallyRelated"); 00917 com.hp.hpl.jena.util.iterator.ExtendedIterator<OntProperty> subPros = (com.hp.hpl.jena.util.iterator.ExtendedIterator<OntProperty>)proExist.listSubProperties(); 00918 00919 while(subPros.hasNext()) { 00920 com.hp.hpl.jena.rdf.model.Statement stm = targetInd.getProperty(subPros.next()); 00921 if(stm != null) { 00922 KnowledgeEngine.ontoDB.removeStatement(stm); 00923 targetInd.removeAll(proExist); 00924 } 00925 } 00926 */ 00927 00928 Property pro = KnowledgeEngine.ontoDB.getProperty(OntoQueryUtil.GlobalNameSpace + "grippedBy"); 00929 00930 targetInd.setPropertyValue(pro, gripInd); 00931 00932 } 00933 catch (Exception e) { 00934 // System.out.println(" ================== " + e.getMessage() + " " + e.toString()); 00935 } 00936 00937 } 00938 00939 public static float getFloatOfStatement(Statement stm) 00940 { 00941 float t = -1000; 00942 try { 00943 t = stm.getFloat(); 00944 } 00945 catch(Exception e) { 00946 System.out.println(e.getMessage()); 00947 } 00948 return t; 00949 } 00950 00951 public static int getIntOfStatement(Statement stm) 00952 { 00953 int t = -1000; 00954 try { 00955 t = stm.getInt(); 00956 } 00957 catch(Exception e) { 00958 System.out.println(e.getMessage()); 00959 } 00960 return t; 00961 } 00962 00963 public static String getStringOfStatement(Statement stm) 00964 { 00965 String ret = ""; 00966 try { 00967 ret = stm.getString(); 00968 } 00969 catch(Exception e) { 00970 System.out.println(" + " + e.toString() + " " + e.getMessage()); 00971 } 00972 return ret; 00973 } 00974 00975 public static String ObjectNameSpace = ""; 00976 public static String GlobalNameSpace = ""; 00977 public static String MapName = ""; 00978 public static String RobotName = "cob3-3"; 00979 public static String Language = "en"; 00980 } 00981