MoveTask.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 org.srs.srs_knowledge.knowledge_engine.*;
00059 import com.hp.hpl.jena.rdf.model.*;
00060 import com.hp.hpl.jena.query.QueryExecutionFactory;
00061 import com.hp.hpl.jena.query.ResultSet;
00062 import com.hp.hpl.jena.query.QueryExecution;
00063 import com.hp.hpl.jena.query.QuerySolution;
00064 
00065 import org.srs.srs_knowledge.task.Task;
00066 
00067 public class MoveTask extends org.srs.srs_knowledge.task.Task
00068 {
00069     public MoveTask(String targetContent) 
00070     {
00071         this.initTask(targetContent);
00072         setTaskType(TaskType.MOVETO_LOCATION);
00073     }
00074     
00075     private void initTask(String targetContent) {
00076         acts = new ArrayList<ActionTuple>();
00077         
00078         setTaskTarget(targetContent);
00079         System.out.println("TASK.JAVA: Created CurrentTask " + "move "
00080                            + targetContent);
00081         constructTask();
00082     }
00083     
00084     protected boolean constructTask() {
00085         return createSimpleMoveTaskNew();
00086     }
00087     
00088     private boolean createSimpleMoveTaskNew() {
00089         // boolean addNewActionTuple(ActionTuple act)
00090         ActionTuple act = new ActionTuple();
00091         
00092         CUAction ca = new CUAction();
00093         GenericAction genericAction = new GenericAction();
00094         
00095         double x = 1;
00096         double y = 1;
00097         double theta = 0;
00098         
00099         if (this.targetContent.charAt(0) == '['
00100             && this.targetContent.charAt(targetContent.length() - 1) == ']') {
00101             StringTokenizer st = new StringTokenizer(targetContent, " [],");
00102             if (st.countTokens() == 3) {
00103                 try {
00104                     x = Double.parseDouble(st.nextToken());
00105                     y = Double.parseDouble(st.nextToken());
00106                     theta = Double.parseDouble(st.nextToken());
00107                     System.out.println(x + "  " + y + " " + theta);
00108                 } catch (Exception e) {
00109                     System.out.println(e.getMessage());
00110                     return false;
00111                 }
00112             }
00113         } else {
00114             System.out.println("======MOVE COMMAND FORMAT=======");
00115             // Ontology queries
00116             String mapNameSpace = OntoQueryUtil.ObjectNameSpace;
00117             String prefix = "PREFIX srs: <http://www.srs-project.eu/ontologies/srs.owl#>\n"
00118                 + "PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>\n"
00119                 + "PREFIX mapNamespacePrefix: <" + mapNameSpace + ">\n";
00120             String queryString = "SELECT ?x ?y ?theta WHERE { "
00121                 + "<" + mapNameSpace + targetContent + ">"
00122                 + " srs:xCoordinate ?x . " + "<" + mapNameSpace + targetContent + ">" + " srs:yCoordinate ?y . "
00123                 + "<" + mapNameSpace + targetContent + ">"
00124                 + " srs:orientationTheta ?theta .}";
00125             //System.out.println(prefix + queryString + "\n");
00126             
00127             if (KnowledgeEngine.ontoDB == null) {
00128                 System.out.println("Ontology Database is NULL");
00129                 return false;
00130             }
00131             
00132             try {
00133                             ArrayList<QuerySolution> rset = KnowledgeEngine.ontoDB.executeQueryRaw(prefix
00134                                                                                    + queryString);
00135                             if (rset.size() == 0) {
00136                                 System.out.println("ERROR: No move target found from database");
00137                                 return false;
00138                             } else if (rset.size() == 1) {
00139                                 System.out
00140                                     .println("INFO: OK info retrieved from DB... ");
00141                                 QuerySolution qs = rset.get(0);
00142                                 x = qs.getLiteral("x").getFloat();
00143                                 y = qs.getLiteral("y").getFloat();
00144                                 theta = qs.getLiteral("theta").getFloat();
00145                                 System.out.println("x is " + x + ". y is  " + y
00146                                                    + ". theta is " + theta);
00147                             } else {
00148                                 System.out.println("WARNING: Multiple options... ");
00149                                 QuerySolution qs = rset.get(0);
00150                                 x = qs.getLiteral("x").getFloat();
00151                                 y = qs.getLiteral("y").getFloat();
00152                                 theta = qs.getLiteral("theta").getFloat();
00153                                 System.out.println("x is " + x + ". y is  " + y
00154                                                    + ". theta is " + theta);
00155                             }
00156                         } catch (Exception e) {
00157                             System.out.println("Exception -->  " + e.getMessage());
00158                             return false;
00159                         }
00160                 }
00161         /*
00162                 genericAction.actionInfo.add("move");
00163                 genericAction.actionInfo.add(Double.toString(x));
00164                 genericAction.actionInfo.add(Double.toString(y));
00165                 genericAction.actionInfo.add(Double.toString(theta));
00166                 */
00167 
00168                 genericAction.jsonActionInfo = SRSJSONParser.encodeMoveAction("move", x, y, theta);
00169                 ca.generic = genericAction;
00170                 ca.actionType = "generic";
00171 
00172                 act.setCUAction(ca);
00173                 act.setActionId(1);
00174                 addNewActionTuple(act);
00175 
00176                 // add finish action __ success
00177 
00178                 act = new ActionTuple();
00179 
00180                 ca = new CUAction();
00181                 genericAction = new GenericAction();
00182                 //genericAction.actionInfo.add("finish_success");
00183                 genericAction.jsonActionInfo = SRSJSONParser.encodeCustomAction("finish_success", null);
00184 
00185                 ca.generic = genericAction;
00186                 ca.actionType = "generic";
00187 
00188                 act.setActionName("finish_success");
00189                 ca.status = 1;
00190 
00191                 act.setCUAction(ca);
00192                 act.setActionId(2);
00193                 act.setParentId(1);
00194                 act.setCondition(true);
00195                 addNewActionTuple(act);
00196 
00197                 // add finish action __ fail
00198 
00199                 act = new ActionTuple();
00200 
00201                 ca = new CUAction();
00202                 genericAction = new GenericAction();
00203                 //genericAction.actionInfo.add("finish_fail");
00204                 genericAction.jsonActionInfo = SRSJSONParser.encodeCustomAction("finish_fail", null);
00205                 ca.generic = genericAction;
00206                 ca.actionType = "generic";
00207 
00208                 act.setActionName("finish_fail");
00209 
00210                 ca.status = -1;
00211                 act.setCUAction(ca);
00212                 act.setActionId(3);
00213                 act.setParentId(1);
00214                 act.setCondition(false);
00215                 addNewActionTuple(act);
00216 
00217                 System.out.println("number of actions: " + acts.size());
00218                 return true;
00219         }
00220 
00221 
00222     public boolean replan(OntologyDB onto, OntoQueryUtil ontoQuery) {
00223         return false;
00224     }
00225 }


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