JSONPrologNode.java
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Lorenz Moesenlechner
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 package edu.tum.cs.ias.knowrob.json_prolog;
00031 
00032 import java.util.*;
00033 import java.io.*;
00034 
00035 import ros.NodeHandle;
00036 import ros.Ros;
00037 import ros.RosException;
00038 import ros.ServiceServer;
00039 import ros.pkg.json_prolog.srv.PrologFinish;
00040 import ros.pkg.json_prolog.srv.PrologNextSolution;
00041 import ros.pkg.json_prolog.srv.PrologQuery;
00042 
00043 
00050 public final class JSONPrologNode {
00051   private Hashtable<String, PrologSolutions> queries;
00052   private boolean hasIncrementalQuery = false; 
00053   private String initPackage="";
00054 
00055   public JSONPrologNode() {
00056     this("");
00057   }
00058 
00059   public JSONPrologNode(String initpkg) {
00060     this.initPackage=initpkg;
00061     queries = new Hashtable<String, PrologSolutions>();
00062   }
00063 
00064 
00065   static private class RospackError extends Exception {
00066     private static final long serialVersionUID = 1L;
00067   }
00068 
00075   private class QueryCallback implements ServiceServer.Callback<PrologQuery.Request, PrologQuery.Response> {
00076 
00077 
00078     @Override
00079     public PrologQuery.Response call(PrologQuery.Request request) {
00080 
00081       PrologQuery.Response response = new PrologQuery.Response();
00082 
00083       if (hasIncrementalQuery ) {
00084         response.ok = false;
00085         response.message = "Already processing an incremental query.";
00086 
00087       } else if ( queries.get(request.id) != null ) {
00088         response.ok = false;
00089         response.message = "Already processing a query with id " + request.id;
00090 
00091       } else {
00092         try {
00093           Ros.getInstance().logDebug("Received query with id " + request.id);
00094 
00095           synchronized(jpl.Query.class) {
00096 
00097             jpl.Query currentQuery = JSONQuery.makeQuery(request.query);
00098             String currentQueryId = request.id;
00099 
00100             if(request.mode == PrologQuery.Request.INCREMENTAL) {
00101               queries.put(currentQueryId, new PrologIncrementalSolutions(currentQuery));
00102               hasIncrementalQuery = true;
00103             }
00104 
00105             else {
00106               queries.put(currentQueryId, new PrologAllSolutions(currentQuery));
00107             }
00108           }
00109           response.ok = true;
00110 
00111         } catch (JSONQuery.InvalidJSONQuery e) {
00112           response.ok = false;
00113           response.message = e.toString();
00114 
00115         } catch (jpl.JPLException e) {
00116           response.ok = false;
00117           response.message = e.getMessage();
00118         }
00119       }
00120       return response;
00121     }
00122   }
00123 
00130   private class SimpleQueryCallback implements
00131   ServiceServer.Callback<PrologQuery.Request, PrologQuery.Response> {
00132 
00133     @Override
00134     public PrologQuery.Response call(PrologQuery.Request request) {
00135 
00136 
00137       PrologQuery.Response response = new PrologQuery.Response();
00138 
00139       try {
00140 
00141         synchronized(jpl.Query.class) {
00142 
00143           if (hasIncrementalQuery ) {
00144             response.ok = false;
00145             response.message = "Already processing an incremental query.";
00146 
00147           } else if (queries!=null && queries.get(request.id) != null ) {
00148             response.ok = false;
00149             response.message = "Already processing a query with id " + request.id;
00150 
00151           } else {
00152             try {
00153               Ros.getInstance().logDebug("Received query with id " + request.id);
00154 
00155               jpl.Query currentQuery = new jpl.Query("expand_goal(("+request.query+"),_Q), call(_Q)");
00156               String currentQueryId = request.id;
00157 
00158               if(request.mode == PrologQuery.Request.INCREMENTAL) {
00159                 queries.put(currentQueryId, new PrologIncrementalSolutions(currentQuery));
00160                 hasIncrementalQuery = true;
00161 
00162               } else {
00163                 queries.put(currentQueryId, new PrologAllSolutions(currentQuery));
00164               }
00165               response.ok = true;
00166 
00167             } catch (jpl.JPLException e) {
00168               response.ok = false;
00169               response.message = e.getMessage();
00170             }
00171           }
00172         }
00173       } catch (Exception e) {
00174         e.printStackTrace();
00175       }
00176 
00177       return response;
00178     }
00179   }
00180 
00181 
00191   private class NextSolutionCallback implements
00192   ServiceServer.Callback<PrologNextSolution.Request, PrologNextSolution.Response> {
00193 
00194 
00195     @Override
00196     public PrologNextSolution.Response call(PrologNextSolution.Request request) {
00197 
00198       PrologNextSolution.Response response = new PrologNextSolution.Response();
00199       try {
00200         synchronized(jpl.Query.class) {
00201 
00202           PrologSolutions currentQuery = queries.get(request.id);
00203           if (currentQuery == null)
00204             response.status = PrologNextSolution.Response.WRONG_ID;
00205 
00206           else if (!currentQuery.hasMoreSolutions()){
00207             response.status = PrologNextSolution.Response.NO_SOLUTION;
00208             removeQuery(request.id);
00209 
00210           } else {
00211             Hashtable<String, jpl.Term> solution = (Hashtable<String, jpl.Term>) currentQuery.nextSolution();
00212             response.solution = JSONQuery.encodeResult(solution).toString();
00213             response.status = PrologNextSolution.Response.OK;
00214           }
00215         }
00216 
00217       } catch (jpl.JPLException e) {
00218         response.solution = e.getMessage();
00219         response.status = PrologNextSolution.Response.QUERY_FAILED;
00220         removeQuery(request.id);
00221 
00222       } catch (Exception e) {
00223         e.printStackTrace();
00224       }
00225 
00226       return response;
00227     }
00228   }
00229 
00230 
00237   private class FinishCallback implements       
00238   ServiceServer.Callback<PrologFinish.Request, PrologFinish.Response> {
00239 
00240     @Override
00241     public PrologFinish.Response call(PrologFinish.Request request) {
00242 
00243       PrologFinish.Response response = new PrologFinish.Response();
00244 
00245       // finish all queries
00246       if (request.id.equals("*")){
00247         Enumeration<String> e = queries.keys();
00248         while(e.hasMoreElements())
00249           removeQuery(e.nextElement());
00250       } else {
00251         removeQuery(request.id);
00252       }
00253       return response;
00254     }
00255   }
00256 
00261   public void execute(String args[]) throws InterruptedException, RosException, IOException, RospackError {
00262 
00263     // init ROS
00264     final Ros ros = Ros.getInstance();
00265     if(!Ros.getInstance().isInitialized()) {
00266       ros.init("json_prolog", false, false, false, args);
00267     }
00268     NodeHandle n = ros.createNodeHandle("~");
00269 
00270     // initialize the Prolog environment
00271     synchronized(jpl.Query.class) {
00272 
00273       initProlog();
00274 
00275       if(n.hasParam("initial_package"))
00276       {
00277         if(!initPackage.equals(""))
00278           ros.logWarn("Initial package has been specivied via command line parameter but the ROS parameter ~initial_package has been set. Using ROS parameter."); 
00279         initPackage = n.getStringParam("initial_package");
00280       }
00281       if(!this.initPackage.equals("")) {
00282         new jpl.Query("ensure_loaded('" + findRosPackage(initPackage)
00283             + "/prolog/init.pl')").oneSolution();
00284       }
00285       if(n.hasParam("goal"))
00286       {
00287           String goal = n.getStringParam("goal");
00288           new jpl.Query(goal).oneSolution();
00289       }
00290     }
00291 
00292     // create services
00293     @SuppressWarnings("unused")
00294     ServiceServer<PrologQuery.Request, PrologQuery.Response, PrologQuery> query_srv = n
00295     .advertiseService("query", new PrologQuery(), new QueryCallback());
00296     @SuppressWarnings("unused")
00297     ServiceServer<PrologQuery.Request, PrologQuery.Response, PrologQuery> simple_query_srv = n
00298     .advertiseService("simple_query", new PrologQuery(), new SimpleQueryCallback());
00299     @SuppressWarnings("unused")
00300     ServiceServer<PrologNextSolution.Request, PrologNextSolution.Response, PrologNextSolution> next_solution_srv = n
00301     .advertiseService("next_solution", new PrologNextSolution(),
00302         new NextSolutionCallback());
00303     @SuppressWarnings("unused")
00304     ServiceServer<PrologFinish.Request, PrologFinish.Response, PrologFinish> finish_srv = n
00305     .advertiseService("finish", new PrologFinish(), new FinishCallback());
00306 
00307 
00308     ros.logInfo("json_prolog initialized and waiting for queries.");
00309     ros.spin();
00310   }
00311 
00312 
00317   public void removeQuery(String id) {
00318 
00319     PrologSolutions query = queries.get(id);
00320 
00321     if(query != null) {
00322       query.close();
00323       queries.remove(id);
00324       if(query instanceof PrologIncrementalSolutions)
00325         hasIncrementalQuery = false;
00326     }
00327   }
00328 
00329 
00337   private static void initProlog() throws IOException, InterruptedException,
00338   RospackError {
00339     Vector<String> pl_args = new Vector<String>(Arrays.asList(jpl.JPL.getDefaultInitArgs()));
00340     pl_args.set(0, "/usr/bin/swipl");
00341     pl_args.add("-G256M");
00342     pl_args.add("-nosignals");
00343     jpl.JPL.setDefaultInitArgs(pl_args.toArray(new String[0]));
00344     jpl.JPL.init();
00345     new jpl.Query("ensure_loaded('" + findRosPackage("rosprolog") + "/prolog/init.pl')").oneSolution();
00346   }
00347 
00357   private static String findRosPackage(String name) throws IOException, InterruptedException, RospackError {
00358 
00359     Process rospack = Runtime.getRuntime().exec("rospack find " + name);
00360     if (rospack.waitFor() != 0)
00361       throw new RospackError();
00362     return new BufferedReader(new InputStreamReader(rospack.getInputStream())).readLine();
00363   }
00364 
00365 
00366 
00367   public static void main(String args[]) {
00368 
00369     try {
00370       // We need to ignore ros specific params such as topic remappings and __name. 
00371       // The easiest way seems to be to just check for := in the string.
00372       if(args.length>0 && !args[0].contains(":="))
00373       {
00374         Ros.getInstance().logWarn("Using deprecated specification of package to load. Please use the corresponding ROS parameter ~initial_package instead");
00375         new JSONPrologNode(args[0]).execute(args);
00376       }
00377       else
00378         new JSONPrologNode().execute(args);
00379 
00380     } catch (Exception e) {
00381       e.printStackTrace();
00382     }
00383   }
00384 }


json_prolog
Author(s): Lorenz Moesenlechner, Moritz Tenorth
autogenerated on Sat Dec 28 2013 17:10:00