MoveAndGraspActionUnit.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 
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.task.Task;
00061 import org.srs.srs_knowledge.knowledge_engine.*;
00062 
00063 import com.hp.hpl.jena.rdf.model.*;
00064 import com.hp.hpl.jena.query.QueryExecutionFactory;
00065 import com.hp.hpl.jena.query.ResultSet;
00066 import com.hp.hpl.jena.query.QueryExecution;
00067 import com.hp.hpl.jena.query.QuerySolution;
00068 import com.hp.hpl.jena.ontology.Individual;
00069 
00070 import org.json.simple.JSONArray;
00071 import org.json.simple.JSONObject;
00072 import org.json.simple.JSONValue;
00073 import org.json.simple.parser.ParseException;
00074 import org.json.simple.parser.JSONParser;
00075 
00080 public class MoveAndGraspActionUnit extends HighLevelActionUnit {
00081 
00082     public MoveAndGraspActionUnit(Pose2D position, String objectClassName, int houseHoldId, String graspConfig) {
00083         init(position, objectClassName, houseHoldId, graspConfig, "");
00084     }
00085 
00086     public MoveAndGraspActionUnit(Pose2D position, String objectClassName, int houseHoldId, String graspConfig, String workspace) {
00087         init(position, objectClassName, houseHoldId, graspConfig, workspace);
00088     }
00089 
00090     private void init(Pose2D position, String objectClassName, int houseHoldId, String graspConfig, String workspace) {
00091             GenericAction ga = new GenericAction();
00092             //ga.actionInfo.add("move");
00093             if(position != null) {
00094                 ga.jsonActionInfo = SRSJSONParser.encodeMoveAction("move", position.x, position.y, position.theta);
00095                 ifBasePoseSet = true;
00096             }
00097             else {
00098                 ga.jsonActionInfo = SRSJSONParser.encodeMoveAction("move", -1000, -1000, -1000);
00099                 ifBasePoseSet = false;
00100             }
00101 
00102             actionUnits.add(ga);
00103 
00104             GenericAction graspAct = new GenericAction();
00105             //graspAct.actionInfo.add("grasp");
00106             objectClassName = (objectClassName == null) ? "" : objectClassName;
00107             ifObjectInfoSet = (objectClassName.trim().equals("")) ? false : true;
00108 
00109             graspConfig = (graspConfig == null) ? "" : graspConfig;
00110             ifObjectInfoSet = true && ((graspConfig.trim().equals("")) ? false : true);
00111             graspAct.jsonActionInfo = SRSJSONParser.encodeGraspAction("grasp", houseHoldId, objectClassName, workspace); 
00112 
00113             actionUnits.add(graspAct);
00114 
00115             //ifObjectPoseSet = true;
00116             //ifParametersSet = ifBasePoseSet && ifObjectInfoSet;
00117             // object not considered here
00118             ifParametersSet = ifBasePoseSet;
00119 
00120             int size = actionUnits.size(); 
00121             nextActionMapIfFail = new int[size];
00122             nextActionMapIfSuccess = new int[size];
00123             
00124             for(int i = 0; i < size; i++) {
00125                 JSONObject tempAct = SRSJSONParser.decodeJsonActionInfo(actionUnits.get(i).jsonActionInfo);
00126                 if(tempAct.get("action").equals("move")) {
00127                     nextActionMapIfSuccess[i] = i + 1;
00128                     nextActionMapIfFail[i] = i + 2;
00129                 }
00130                 else if(tempAct.get("action").equals("grasp")) {
00131                     nextActionMapIfSuccess[i] = COMPLETED_SUCCESS;    // 
00132                     nextActionMapIfFail[i] = i + 1;
00133                 }
00134                 if(nextActionMapIfFail[i] >= size) {
00135                     // out of bound, means this is the last step in this action unit. so -1 means there is no further solution to the current task within this actionunit
00136                     nextActionMapIfFail[i] = COMPLETED_FAIL;  
00137                 }           
00138         }
00139     }
00140     
00141     @Override
00142     public String getActionType() {
00143         actionType = "MoveAndGrasp";
00144         return actionType;
00145     }
00146 
00147     @Override
00148     public int getNextCUActionIndex(boolean statusLastStep) {
00149         if(currentActionInd == -1) {
00150             return 0;
00151         }
00152 
00153         if ( currentActionInd >= 0 && currentActionInd < actionUnits.size() ) {
00154 
00155             if(statusLastStep) {
00156                 
00157                 System.out.println("NEXT ACTION IND (if Successful): " + nextActionMapIfSuccess[currentActionInd]);
00158                 return nextActionMapIfSuccess[currentActionInd];
00159             }
00160             else {
00161                 System.out.println("NEXT ACTION IND (if Failed): " + nextActionMapIfFail[currentActionInd]);
00162                 return nextActionMapIfFail[currentActionInd];
00163             }
00164         }
00165         else {
00166             return INVALID_INDEX;
00167         }
00168     }
00169 
00170     @Override
00171     public boolean setParameters(String action, String para, String reservedParam) {
00172         if(action.equals("move")) {
00173             setBasePose(para);
00174         }
00175         else if(action.equals("grasp")) {
00176             setGraspInfo(para);
00177         }
00178         return ifParametersSet;
00179     }
00180 
00181     private void setBasePose(String jsonPose) {
00182 
00183         GenericAction nga = new GenericAction();
00184         nga.jsonActionInfo = jsonPose;
00185         actionUnits.set(0, nga);
00186         ifBasePoseSet = true;
00187         ifParametersSet = ifBasePoseSet && ifObjectInfoSet;
00188         
00189     }
00190 
00191     private void setGraspInfo(String jsonInfo) {
00192 
00193         GenericAction nga = new GenericAction();
00194         nga.jsonActionInfo = jsonInfo;
00195         actionUnits.set(1, nga);
00196         ifObjectInfoSet = true;
00197         ifParametersSet = ifBasePoseSet && ifObjectInfoSet;
00198 
00199     }
00200 
00201     private boolean setObjectPose(ArrayList<String> objPose) {
00202         return false;
00203     }
00204 
00205     @Override
00206     public boolean ifParametersSet() {
00207         ifParametersSet = ifBasePoseSet && ifObjectInfoSet;
00208         return ifParametersSet;
00209     }
00210 
00211     private boolean ifBasePoseSet = false;
00212     private boolean ifObjectInfoSet = false;
00213 }


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