SRSJSONParser.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.knowledge_engine;
00052 
00053 import java.io.*;
00054 import java.util.ArrayList; 
00055 import java.util.NoSuchElementException;
00056 import java.util.HashSet;
00057 import java.util.Iterator;
00058 import java.util.Map;
00059 
00060 import java.io.StringWriter;
00061 import java.math.BigInteger;
00062 
00063 import ros.pkg.srs_knowledge.msg.*;
00064 import ros.pkg.srs_msgs.msg.SRSSpatialInfo;
00065 import ros.pkg.srs_knowledge.srv.PlanNextAction;
00066 import ros.pkg.srs_knowledge.srv.TaskRequest;
00067 import ros.pkg.srs_knowledge.srv.GetObjectsOnMap;
00068 import ros.pkg.srs_knowledge.srv.GetWorkspaceOnMap;
00069 import org.srs.srs_knowledge.task.*;
00070 import ros.pkg.geometry_msgs.msg.Pose2D;
00071 import ros.pkg.geometry_msgs.msg.Pose;
00072 
00073 import org.srs.srs_knowledge.utils.*;
00074 
00075 import org.json.simple.JSONArray;
00076 import org.json.simple.JSONObject;
00077 import org.json.simple.JSONValue;
00078 import org.json.simple.parser.ParseException;
00079 import org.json.simple.parser.JSONParser;
00080 
00081 public class SRSJSONParser
00082 {
00083 
00088     public static Task parseJSONToTask(String encodedTask) {
00089         Task t = null;
00090         // {"tasks":[{"time_schedule":1263798000000,"task":"move","destination":{"pose2d_string":"[0 1 3.14]"}}],"initializer":{"device_type":"ui_loc","device_id":"ui_loc_0001"}}
00091         System.out.println("=> decode json: " + encodedTask);
00092 
00093         /*
00094         Object obj = JSONValue.parse(encodedTask);
00095         if (obj == null) {
00096             System.out.println("Parsing Json error");
00097             return null;
00098         }
00099         */
00100         JSONParser parser = new JSONParser();
00101         JSONObject task = new JSONObject();
00102         try {
00103             Object obj = parser.parse(encodedTask);
00104             task = (JSONObject)obj;         
00105         }
00106         catch(ParseException pe) {
00107             System.out.println("Parsing Json error at position: " + pe.getPosition());
00108             System.out.println(pe);
00109             return null;
00110         }
00111         
00112         String taskName = (String)task.get("task");
00113         if(taskName.equals("move")) {
00114             t = parseJSONToMoveTask(task);
00115         }
00116         else if(taskName.equals("get")) {
00117             t = parseJSONToGetTask(task);
00118         }
00119         else if(taskName.equals("fetch")) {
00120             t = parseJSONToFetchTask(task);
00121         }
00122         else if(taskName.equals("search")) {
00123             t = parseJSONToSearchTask(task);
00124         }
00125         else if(taskName.equals("deliver")) {
00126             t = parseJSONToGetTask(task);
00127         }
00128         else if(taskName.equals("stop")) {
00129             t = parseJSONToMoveTask(task);
00130         }
00131         else {
00132             System.out.println("Invalid task type -- error when decoding json task format");
00133             return t;
00134         }
00135 
00136         return t;
00137     }
00138     
00139     public static MoveTask parseJSONToMoveTask(JSONObject task) {
00140         MoveTask mt = null;
00141         
00142         java.lang.Number time = (java.lang.Number)task.get("time_schedule");
00143         JSONObject destination = (JSONObject)task.get("destination");   
00144         String dest = "";
00145         if(destination.containsKey("predefined_pose")) {
00146             dest = (String)destination.get("predefined_pose");
00147         }
00148         else if(destination.containsKey("pose2d_string")) {
00149             dest = (String)destination.get("pose2d_string");
00150         }
00151         else if(destination.containsKey("pose2d")) {
00152             JSONObject pose = (JSONObject)destination.get("pose2d");
00153             double x = ((Number)(pose.get("x"))).doubleValue();
00154             double y = ((Number)(pose.get("y"))).doubleValue();
00155             double theta = ((Number)(pose.get("theta"))).doubleValue();
00156             dest = "[" + x + " " + y + " " + theta + "]";
00157         }
00158         else {
00159             return null;
00160         }
00161 
00162         mt = new MoveTask(dest);
00163         return mt;
00164     }
00165 
00166     public static GetObjectTask parseJSONToGetTask(JSONObject task) {
00167         GetObjectTask got = null;
00168         // {"task":"get","object":{"object_type":"Book"}}
00169         try {
00170         JSONObject obj = (JSONObject)task.get("object");
00171         String objectType = (String)(obj.get("object_type"));
00172         
00173         // This should be passed by dm, not read from param server... to minimize the dependence on other packages
00174         String graspType = (String)(task.get("grasping_type"));
00175         ConfigInfo.GraspType graspingType = ConfigInfo.GraspType.MOVE_AND_GRASP;
00176         if(graspType.equals("Simple")) {
00177             graspingType = ConfigInfo.GraspType.MOVE_AND_GRASP;
00178         }
00179         else if(graspType.equals("Planned")) {
00180             graspingType = ConfigInfo.GraspType.JUST_GRASP;
00181         }
00182         System.out.println("---->>> " + graspType);
00183       
00184         got = new GetObjectTask(objectType, graspingType);
00185         }
00186         catch(Exception e) {
00187             System.out.println(e.toString());
00188         }
00189         
00190         return got;
00191     }
00192 
00193     public static FetchObjectTask parseJSONToFetchTask(JSONObject task) {
00194         FetchObjectTask fot = null;
00195         // {"task":"get","object":{"object_type":"Book"}}
00196         try {
00197             JSONObject obj = (JSONObject)task.get("object");
00198             String objectType = (String)(obj.get("object_type"));
00199             
00200             //String graspType = (String)(obj.get("grasping_type"));
00201             JSONObject deliverDest = (JSONObject)task.get("deliver_destination");
00202             
00203             String dest = "";
00204             if(deliverDest.containsKey("predefined_pose")) {
00205                 dest = (String)deliverDest.get("predefined_pose");
00206             }
00207             else if(deliverDest.containsKey("pose2d_string")) {
00208                 dest = (String)deliverDest.get("pose2d_string");
00209             }
00210             else if(deliverDest.containsKey("pose2d")) {
00211                 JSONObject pose = (JSONObject)deliverDest.get("pose2d");
00212                 double x = ((Number)(pose.get("x"))).doubleValue();
00213                 double y = ((Number)(pose.get("y"))).doubleValue();
00214                 double theta = ((Number)(pose.get("theta"))).doubleValue();
00215                 dest = "[" + x + " " + y + " " + theta + "]";
00216             }
00217             else {
00218                 return null;
00219             }
00220             // This should be passed by dm, not read from param server... to minimize the dependence on other packages
00221             String graspType = (String)task.get("grasping_type");
00222             System.out.println("graspType -->  " + graspType);
00223 
00224             ConfigInfo.GraspType graspingType = ConfigInfo.GraspType.MOVE_AND_GRASP;
00225             if(graspType.equals("Simple")) {
00226                 graspingType = ConfigInfo.GraspType.MOVE_AND_GRASP;
00227             }
00228             else if(graspType.equals("Planned")) {
00229                 graspingType = ConfigInfo.GraspType.JUST_GRASP;
00230             }
00231 
00232             System.out.println("graspingType " + graspingType);
00233             fot = new FetchObjectTask(objectType, dest, graspingType);
00234         }
00235         catch(Exception e) {
00236             System.out.println(e.toString());
00237         }
00238         
00239         return fot;
00240     }
00241 
00242     public static SearchObjectTask parseJSONToSearchTask(JSONObject task) {
00243         SearchObjectTask sot = null;
00244 
00245         try {
00246             JSONObject obj = (JSONObject)task.get("object");
00247             String objectType = (String)(obj.get("object_type"));
00248             
00249             sot = new SearchObjectTask(objectType);
00250         }
00251         catch(Exception e) {
00252             System.out.println(e.toString());
00253         }       
00254         return sot;
00255     }
00256 
00257     public static StopTask parseJSONToStopTask(JSONObject task) {
00258         StopTask st = new StopTask();
00259         return st;
00260     }
00261 
00262     public static String encodeMoveAction(String action, double x, double y, double theta) {
00263         JSONObject moveAct = new JSONObject();
00264         moveAct.put("action", action);
00265 
00266         JSONObject dest = new JSONObject();
00267         dest.put("x", x);
00268         dest.put("y", y);
00269         dest.put("theta", theta);
00270 
00271         JSONObject pos = new JSONObject();
00272         pos.put("pose2d", dest);
00273 
00274         moveAct.put("destination", pos);
00275         try {
00276             StringWriter out = new StringWriter();
00277             moveAct.writeJSONString(out);
00278             String jsonText = out.toString();
00279             return jsonText;
00280         }
00281         catch(IOException ie) {
00282             System.out.println("IO Exception when writing JSON STRING");
00283             System.out.println(ie.getMessage());
00284             return "";
00285         }
00286     }
00287 
00288     public static String encodeCustomAction(String action, Map<String, Object> content) {
00289         JSONObject act = new JSONObject();
00290         act.put("action", action);
00291         try {
00292             StringWriter out = new StringWriter();
00293             act.writeJSONString(out);
00294             String jsonText = out.toString();
00295             return jsonText;
00296         }
00297         catch(IOException ie) {
00298             System.out.println("IO Exception when writing JSON STRING");
00299             System.out.println(ie.getMessage());
00300             return "";
00301         }
00302     }
00303 
00304     public static String encodePutOnTrayAction(String action, String moveConfig) {
00305         JSONObject act = new JSONObject();
00306         act.put("action", action);
00307         act.put("move_config", moveConfig);
00308         try {
00309             StringWriter out = new StringWriter();
00310             act.writeJSONString(out);
00311             String jsonText = out.toString();
00312             return jsonText;
00313         }
00314         catch(IOException ie) {
00315             System.out.println("IO Exception when writing JSON STRING");
00316             System.out.println(ie.getMessage());
00317             return "";
00318         }
00319     }
00320 
00321     public static String encodeDetectAction(String action, int hhId, String objectType, String workspace) {
00322         JSONObject obj = new JSONObject();
00323         obj.put("object_type", objectType);
00324         obj.put("object_id", hhId);
00325         obj.put("workspace", workspace);
00326 
00327         JSONObject detAct = new JSONObject();
00328         detAct.put("object", obj);
00329         detAct.put("action", action);
00330 
00331         try {
00332             StringWriter out = new StringWriter();
00333             detAct.writeJSONString(out);
00334             String jsonText = out.toString();
00335             return jsonText;
00336         }
00337         catch(IOException ie) {
00338             System.out.println("IO Exception when writing JSON STRING");
00339             System.out.println(ie.getMessage());
00340             return "";
00341         }
00342     }
00343     
00344     public static String encodeGraspAction(String action, int hhId, String objectType, String workspace) {
00345         JSONObject obj = new JSONObject();
00346         obj.put("object_type", objectType);
00347         obj.put("object_id", hhId);
00348         if(workspace != null) {
00349             obj.put("workspace", workspace);
00350         }
00351         else { 
00352             obj.put("workspace", "");
00353         }
00354 
00355         JSONObject detAct = new JSONObject();
00356         detAct.put("object", obj);
00357         detAct.put("action", action);
00358 
00359         try {
00360             StringWriter out = new StringWriter();
00361             detAct.writeJSONString(out);
00362             String jsonText = out.toString();
00363             return jsonText;
00364         }
00365         catch(IOException ie) {
00366             System.out.println("IO Exception when writing JSON STRING");
00367             System.out.println(ie.getMessage());
00368             return "";
00369         }
00370     }
00371 
00372     public static String encodeCheckObjectAction(String action, String objectType, double x, double y, double z, double orix, double oriy, double oriz, double oriw, double l, double w, double h) {
00373         JSONObject obj = new JSONObject();
00374         obj.put("object_type", objectType);
00375 
00376         JSONObject pose = new JSONObject();
00377         JSONObject position = new JSONObject();
00378         position.put("x", x);
00379         position.put("y", y);
00380         position.put("x", z);
00381         pose.put("position", position);
00382        
00383         JSONObject orientation = new JSONObject();
00384         orientation.put("x", orix);
00385         orientation.put("y", oriy);
00386         orientation.put("z", oriz);
00387         orientation.put("w", oriw);
00388         pose.put("orientation", orientation);
00389         
00390         JSONObject dim = new JSONObject();
00391         dim.put("l", l);
00392         dim.put("w", w);
00393         dim.put("h", h);
00394         
00395         obj.put("pose", pose);
00396         obj.put("dimension", dim);
00397 
00398         JSONObject checkAct = new JSONObject();
00399         checkAct.put("object", obj);
00400         checkAct.put("action", action);
00401 
00402         try {
00403             StringWriter out = new StringWriter();
00404             checkAct.writeJSONString(out);
00405             String jsonText = out.toString();
00406             return jsonText;
00407         }
00408         catch(IOException ie) {
00409             System.out.println("IO Exception when writing JSON STRING");
00410             System.out.println(ie.getMessage());
00411             return "";
00412         }
00413     }
00414 
00415     public static String encodeObjectCategoryInfo(HashSet<String> items) {
00416         JSONArray cats = new JSONArray();
00417         Iterator<String> it = items.iterator();
00418         while (it.hasNext()) {
00419             cats.add(it.next());
00420         }
00421 
00422         JSONObject cat = new JSONObject();
00423         cat.put("object_categories", cats);
00424 
00425         try {
00426             StringWriter out = new StringWriter();
00427             cat.writeJSONString(out);
00428             String jsonText = out.toString();
00429             return jsonText;
00430         }
00431         catch(IOException ie) {
00432             System.out.println("IO Exception when writing JSON STRING");
00433             System.out.println(ie.getMessage());
00434             return "";
00435         }
00436     }
00437 
00438 
00439     public static JSONObject decodeJsonActionInfo(String jsonActionInfo) {
00440         JSONParser parser = new JSONParser();
00441         JSONObject actionInfo = new JSONObject();
00442         try {
00443             Object obj = parser.parse(jsonActionInfo);
00444             actionInfo = (JSONObject)obj;           
00445         }
00446         catch(ParseException pe) {
00447             System.out.println("Parsing Json error at position: " + pe.getPosition());
00448             System.out.println(pe);
00449             return null;
00450         }
00451         
00452         return actionInfo;
00453     }
00454 
00455     public static String encodeObjectProperties(Map<String, String> pro)
00456     {
00457         return JSONObject.toJSONString(pro);
00458     }
00459 
00460 }


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