MoveAndCheckWorkspaceActionUnit.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 
00060 import org.srs.srs_knowledge.task.*;
00061 import org.srs.srs_knowledge.knowledge_engine.*;
00062 import ros.pkg.srs_symbolic_grounding.srv.*;
00063 
00064 import org.json.simple.JSONArray;
00065 import org.json.simple.JSONObject;
00066 import org.json.simple.JSONValue;
00067 import org.json.simple.parser.ParseException;
00068 import org.json.simple.parser.JSONParser;
00069 
00070 import ros.pkg.srs_msgs.msg.SRSSpatialInfo;
00071 
00076 public class MoveAndCheckWorkspaceActionUnit extends HighLevelActionUnit {
00077 
00078     public MoveAndCheckWorkspaceActionUnit(ArrayList<Pose2D> positions, String objectClassName, SRSSpatialInfo targetSurface) {
00079         for(Pose2D position:positions) {
00080             GenericAction ga = new GenericAction();
00081 
00082             ga.jsonActionInfo = SRSJSONParser.encodeMoveAction("move", position.x, position.y, position.theta);
00083             actionUnits.add(ga);
00084 
00085             GenericAction detAct = new GenericAction();
00086 
00087             detAct.jsonActionInfo = SRSJSONParser.encodeCheckObjectAction("check", objectClassName, targetSurface.pose.position.x, targetSurface.pose.position.y, targetSurface.pose.position.z, targetSurface.pose.orientation.x, targetSurface.pose.orientation.y, targetSurface.pose.orientation.z, targetSurface.pose.orientation.w, targetSurface.l, targetSurface.w, targetSurface.h);
00088 
00089             actionUnits.add(detAct);
00090         }
00091 
00092         // this actionunit is always set with sufficient parameters
00093         ifParametersSet = true;
00094 
00095         int size = actionUnits.size(); 
00096         nextActionMapIfFail = new int[size];
00097         nextActionMapIfSuccess = new int[size];
00098 
00099         for(int i = 0; i < size; i++) {
00100             //if(actionUnits.get(i).actionInfo.get(0).equals("move")) {
00101             JSONObject tempAct = SRSJSONParser.decodeJsonActionInfo(actionUnits.get(i).jsonActionInfo);
00102             if(tempAct.get("action").equals("move")) {
00103                 nextActionMapIfSuccess[i] = i + 1;
00104                 nextActionMapIfFail[i] = i + 2;
00105                 System.out.println("move ----   " + i);
00106             }
00107             else if(tempAct.get("action").equals("check")) {
00108                 nextActionMapIfSuccess[i] = COMPLETED_SUCCESS;    // 
00109                 nextActionMapIfFail[i] = i + 1;
00110                 System.out.println("check ----   " + i);
00111             }
00112             if(nextActionMapIfFail[i] >= size) {
00113                 // 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
00114                 nextActionMapIfFail[i] = COMPLETED_FAIL;  
00115                 System.out.println("overflow ----   " + i);
00116             }       
00117         }
00118     }
00119 
00120     @Override
00121     public String getActionType() {
00122         actionType = "MoveAndCheckWorkspace";
00123         return actionType;
00124     }
00125 
00126     @Override
00127     public boolean ifParametersSet() {
00128         return ifParametersSet;
00129     }
00130 
00131     @Override
00132     public boolean setParameters(String action, String para, String reservedParam) {
00133         return ifParametersSet;
00134     }
00135 }


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