$search
00001 package edu.tum.cs.ias.knowrob.mod_dialog; 00002 00003 import instruction.gui.EHowInstructionPanel; 00004 00005 import java.awt.BorderLayout; 00006 import java.awt.Color; 00007 import java.awt.event.KeyEvent; 00008 import java.awt.event.MouseListener; 00009 import java.awt.event.MouseMotionListener; 00010 import java.awt.event.WindowAdapter; 00011 import java.awt.event.WindowEvent; 00012 import java.io.BufferedReader; 00013 import java.io.File; 00014 import java.io.IOException; 00015 import java.io.InputStreamReader; 00016 import java.lang.reflect.Constructor; 00017 import java.lang.reflect.InvocationTargetException; 00018 import java.lang.reflect.Modifier; 00019 import java.net.URL; 00020 import java.util.ArrayList; 00021 import java.util.Arrays; 00022 import java.util.Enumeration; 00023 import java.util.HashMap; 00024 import java.util.HashSet; 00025 import java.util.Hashtable; 00026 import java.util.List; 00027 import java.util.Vector; 00028 00029 import javax.swing.JFrame; 00030 00031 import processing.core.PApplet; 00032 import processing.core.PFont; 00033 import ros.NodeHandle; 00034 import ros.Publisher; 00035 import ros.Ros; 00036 import ros.RosException; 00037 import ros.ServiceClient; 00038 import ros.Subscriber; 00039 import ros.pkg.mary_tts.srv.SetMaryVoice; 00040 import controlP5.CheckBox; 00041 import controlP5.ControlEvent; 00042 import controlP5.ControlFont; 00043 import controlP5.ControlP5; 00044 import controlP5.Textarea; 00045 import controlP5.Textfield; 00046 import edu.tum.cs.ias.knowrob.json_prolog.Prolog; 00047 import edu.tum.cs.ias.knowrob.json_prolog.PrologBindings; 00048 import edu.tum.cs.ias.knowrob.json_prolog.PrologQueryProxy; 00049 import edu.tum.cs.ias.knowrob.json_prolog.PrologValue; 00050 import edu.tum.cs.ias.knowrob.mod_dialog.queries.DialogQuery; 00051 00052 00053 00054 public class DialogModule extends PApplet implements MouseListener, MouseMotionListener { 00055 00056 private static final long serialVersionUID = 4575739930038583994L; 00057 00058 public static String TOP_LEVEL_STATE = "init"; 00059 protected String state = TOP_LEVEL_STATE; 00060 protected HashSet<String> registeredStates = new HashSet<String>(); 00061 00062 private static HashMap<String, String> prologToEnglish; 00063 private static HashMap<String, String> englishToProlog; 00064 00065 protected String currentObject=""; 00066 00067 public JFrame img_window; 00068 public ImageViewerApplet iviewer; 00069 public JFrame ehow_window; 00070 public EHowInstructionPanel ehow_panel; 00071 00072 protected boolean useSpeechInput = true; 00073 protected static boolean localProlog = false; 00074 00075 00076 static { 00077 // initialize the internal mappings en <-> pl 00078 setMappings(); 00079 } 00080 00081 ArrayList<DialogQuery> queries; 00082 00083 // components 00084 public static ControlP5 controlP5; 00085 ControlFont font; 00086 CheckBox checkbox; 00087 00091 static Ros ros; 00092 public static NodeHandle n; 00093 00094 static Subscriber.QueueingCallback<ros.pkg.std_msgs.msg.String> callback; 00095 00096 00097 Publisher<ros.pkg.std_msgs.msg.String> mary_pub; 00098 00099 protected static void initRos() { 00100 00101 ros = Ros.getInstance(); 00102 00103 if(!Ros.getInstance().isInitialized()) { 00104 ros.init("knowrob_dialog_applet"); 00105 } 00106 n = ros.createNodeHandle(); 00107 00108 callback = new Subscriber.QueueingCallback<ros.pkg.std_msgs.msg.String>(); 00109 } 00110 00111 00112 @SuppressWarnings("unchecked") 00113 public void setup() { 00114 00115 size(550, 400, P2D); 00116 // this.frame.setTitle("DialogModule"); 00117 background(20, 20, 20); 00118 // this.frame.setBackground(new Color(20, 20, 20)); 00119 00120 frameRate(10); 00121 initControlP5(); 00122 draw(); 00123 00124 // load the classes for the different queries 00125 try { 00126 00127 queries = new ArrayList<DialogQuery>(); 00128 for(Class cl : getClasses("edu.tum.cs.ias.knowrob.mod_dialog.queries")) { 00129 00130 if(Modifier.isAbstract(cl.getModifiers())) 00131 continue; 00132 // System.out.println(cl.getSimpleName()); 00133 00134 try { 00135 Constructor<DialogQuery> constructor = cl.getConstructor(DialogModule.class); 00136 DialogQuery q; 00137 q = constructor.newInstance(this); 00138 queries.add(q); 00139 } catch (SecurityException e) { 00140 e.printStackTrace(); 00141 } catch (NoSuchMethodException e) { 00142 e.printStackTrace(); 00143 } catch (IllegalArgumentException e) { 00144 e.printStackTrace(); 00145 } catch (InstantiationException e) { 00146 e.printStackTrace(); 00147 } catch (IllegalAccessException e) { 00148 e.printStackTrace(); 00149 } catch (InvocationTargetException e) { 00150 e.printStackTrace(); 00151 } 00152 } 00153 00154 } catch (ClassNotFoundException e) { 00155 e.printStackTrace(); 00156 } catch (IOException e) { 00157 e.printStackTrace(); 00158 } 00159 00160 00161 // start ROS environment 00162 initRos(); 00163 00164 Thread speechIn = new Thread( new SpeechInListenerThread() ); 00165 speechIn.start(); 00166 00167 Thread answerQueries = new Thread( new AnswerQueriesThread(this) ); 00168 answerQueries.start(); 00169 00170 try { 00171 mary_pub = n.advertise("/mary/tts", new ros.pkg.std_msgs.msg.String(), 100); 00172 00173 } catch (RosException e) { 00174 e.printStackTrace(); 00175 } 00176 00177 00178 // setup and pre-initialize the ehow window 00179 ehow_window = new JFrame(); 00180 ehow_window.setVisible( false ); 00181 ehow_window.setSize( 1100, 800 ); 00182 ehow_window.setTitle( "Plan Importer GUI" ); 00183 ehow_window.setLocation( 400, 300 ); 00184 ehow_window.setBackground(new Color(20, 20, 20)); 00185 00186 ehow_window.addWindowListener( new WindowAdapter() { 00187 public void windowClosing( WindowEvent we ) { 00188 //System.exit( 0 ); 00189 ehow_window.setVisible(false); 00190 } } ); 00191 00192 ehow_panel = new EHowInstructionPanel(); 00193 ehow_window.add( ehow_panel ); 00194 ehow_panel.init(); 00195 ehow_panel.revalidate(); 00196 00197 if(localProlog) { 00198 initProlog(); 00199 } 00200 00201 // load the initial Prolog environment 00202 // executeJSONPrologQuery("comp_cop:odufinder_listener(_)"); 00203 // executeQuery("register_ros_package('ias_semantic_map')"); 00204 // executeQuery("register_ros_package('ias_prolog_addons')"); 00205 // executeQuery("use_module(library('comp_similarity'))"); 00206 00207 } 00208 00209 public void draw() { 00210 00211 // this.frame.setBackground(new Color(20, 20, 20)); 00212 background(20, 20, 20); 00213 controlP5.draw(); 00214 } 00215 00216 00220 // 00221 // STATE MANAGEMENT 00222 // 00223 00224 public String getCurrentState() { 00225 return state; 00226 } 00227 00228 public boolean stateExists(String state) { 00229 return state.equals(TOP_LEVEL_STATE) || registeredStates.contains(state); 00230 } 00231 00232 public void registerState(String state) { 00233 if(stateExists(state)) 00234 throw new IllegalArgumentException("State already registered"); 00235 registeredStates.add(state); 00236 } 00237 00238 public void setState(String state) { 00239 if(!stateExists(state)) 00240 throw new IllegalArgumentException("Tried to set invalid state"); 00241 this.state = state; 00242 } 00243 00244 public void setTopLevelState() { 00245 setState(TOP_LEVEL_STATE); 00246 } 00247 00251 // 00252 // QUERY ANSWERING 00253 // 00254 00255 00256 String answer_query(String q) { 00257 00258 String res = null; 00259 try{ 00260 00261 // iterate over query classes until one of them matches the query string 00262 for(DialogQuery query : queries) { 00263 res = query.process(q); 00264 if(res != null) { 00265 System.out.println(query.getClass().getSimpleName() + " returned '" + res + "'"); 00266 return res; 00267 } 00268 } 00269 00270 // default: 00271 return "I don't know.\n"; 00272 00273 } catch(Exception e) { 00274 e.printStackTrace(); 00275 return "I don't know.\n"; 00276 } 00277 } 00278 00279 00285 public static HashMap<String, Vector<PrologValue>> executeJSONPrologQuery(String query) { 00286 00287 if(localProlog) { 00288 return executeLocalPrologQuery(query); 00289 } 00290 00291 System.out.println(query); 00292 00293 HashMap<String, Vector<PrologValue>> result = new HashMap< String, Vector<PrologValue> >(); 00294 00295 Prolog pl = new Prolog("/json_prolog"); 00296 PrologQueryProxy bdgs = pl.query("expand_goal(("+query+"),_9), call(_9)"); 00297 00298 // return null if there are no solutions (empty hash table == true) 00299 if(!bdgs.iterator().hasNext()) { 00300 return null; 00301 } 00302 00303 for(PrologBindings bdg : bdgs) { 00304 00305 for(String key : bdg.getBdgs_().keySet()) { 00306 00307 if(!result.containsKey(key)) { 00308 result.put(key, new Vector<PrologValue>()); 00309 } 00310 result.get(key).add(bdg.getBdgs_().get(key)); 00311 } 00312 } 00313 bdgs.finish(); 00314 00315 return result; 00316 } 00317 00318 public static HashMap<String, Vector<PrologValue>> executeLispPrologQuery(String query) { 00319 00320 System.out.println(query); 00321 00322 HashMap<String, Vector<PrologValue>> result = new HashMap< String, Vector<PrologValue> >(); 00323 00324 Prolog pl = new Prolog("/execution_trace_server"); 00325 PrologQueryProxy bdgs = pl.query(query); 00326 00327 // return null if there are no solutions (empty hash table == true) 00328 if(!bdgs.iterator().hasNext()) { 00329 return null; 00330 } 00331 00332 for(PrologBindings bdg : bdgs) { 00333 00334 for(String key : bdg.getBdgs_().keySet()) { 00335 00336 if(!result.containsKey(key)) { 00337 result.put(key, new Vector<PrologValue>()); 00338 } 00339 result.get(key).add(bdg.getBdgs_().get(key)); 00340 } 00341 } 00342 bdgs.finish(); 00343 00344 return result; 00345 } 00346 00347 00351 // 00352 // ROS STUFF 00353 // 00354 00355 00356 public static class SpeechInListenerThread implements Runnable { 00357 00358 @Override public void run() { 00359 00360 try { 00361 00362 n.advertise("/knowrob/speech_in", new ros.pkg.std_msgs.msg.String(), 100); 00363 Subscriber<ros.pkg.std_msgs.msg.String> sub = n.subscribe("/knowrob/speech_in", new ros.pkg.std_msgs.msg.String(), callback, 10); 00364 00365 n.spin(); 00366 sub.shutdown(); 00367 00368 } catch(RosException e) { 00369 e.printStackTrace(); 00370 } 00371 } 00372 } 00373 00374 00375 public static class AnswerQueriesThread implements Runnable { 00376 00377 DialogModule dialog_module; 00378 00379 public AnswerQueriesThread(DialogModule parent) { 00380 this.dialog_module=parent; 00381 } 00382 00383 @Override public void run() { 00384 00385 try { 00386 00387 ros.pkg.std_msgs.msg.String res; 00388 while (n.isValid()) { 00389 00390 res = callback.pop(); 00391 00392 if(((CheckBox) controlP5.getGroup("speech_in")).getState(0)) 00393 dialog_module.handleTextQuery(res.data); 00394 00395 } 00396 00397 } catch (InterruptedException e) { 00398 e.printStackTrace(); 00399 } 00400 } 00401 } 00402 00403 00404 00405 00409 // 00410 // QUERY UTILITY METHODS 00411 // 00412 00413 public void setVoice(String voice) { 00414 00415 try { 00416 ServiceClient<SetMaryVoice.Request, SetMaryVoice.Response, SetMaryVoice> cl = 00417 n.serviceClient("/mary_tts/set_voice", new SetMaryVoice()); 00418 SetMaryVoice.Request req = new SetMaryVoice.Request(); 00419 req.voice_name=voice; 00420 cl.call(req); 00421 } catch (RosException e) { 00422 } 00423 } 00424 00425 00426 public String getCurrentObject() { 00427 return currentObject; 00428 } 00429 00430 00431 public void setCurrentObject(String currentObject) { 00432 this.currentObject = currentObject; 00433 } 00434 00435 public static String removeSingleQuotes(String str) { 00436 if(str.startsWith("'")) 00437 str = str.substring(1); 00438 00439 if(str.endsWith("'")) 00440 str = str.substring(0, str.length()-1); 00441 return str; 00442 } 00443 00444 protected static String startToUpperCase(String q) { 00445 return q.substring(0,1).toUpperCase() + q.substring(1, q.length()); 00446 } 00447 00448 public static String toProlog(String enString) { 00449 00450 if(englishToProlog.containsKey(enString)) { 00451 return "knowrob:'"+englishToProlog.get(enString)+"'"; 00452 00453 } else if( enString.startsWith("cupboard") || 00454 enString.startsWith("oven") || 00455 enString.startsWith("refrigerator") || 00456 enString.startsWith("diswasher") || 00457 enString.startsWith("table") || 00458 enString.startsWith("kitchentable") ) { 00459 return "ias_map:'"+enString+"'"; 00460 00461 } else { 00462 return "knowrob:'"+startToUpperCase(enString)+"'"; 00463 } 00464 } 00465 00466 public static String toEnglish(String plString) { 00467 00468 if(prologToEnglish.containsKey(plString)) { 00469 return prologToEnglish.get(plString); 00470 00471 } else if(plString.contains("#")) { // strip URL 00472 return removeSingleQuotes(plString.split("#")[1]); 00473 00474 } else if(plString.contains(":")) { // strip namespace 00475 return removeSingleQuotes(plString.split(":")[1]); 00476 } else { 00477 return plString; 00478 } 00479 } 00480 00481 private static void setMappings() { 00482 00483 prologToEnglish = new HashMap<String, String>(); 00484 englishToProlog = new HashMap<String, String>(); 00485 00486 addMapping("type", "has the type"); 00487 addMapping("depthOfObject", "has the depth"); 00488 addMapping("widthOfObject", "has the width"); 00489 addMapping("heightOfObject", "has the height"); 00490 addMapping("properPhysicalPartTypes", "has a part"); 00491 addMapping("describedInMap", "is part of the map"); 00492 addMapping("subClassOf", "is a sub-class of"); 00493 00494 } 00495 00496 private static void addMapping(String pl, String en) { 00497 prologToEnglish.put(pl, en); 00498 englishToProlog.put(en, pl); 00499 } 00500 00501 00502 00503 00507 // 00508 // USER INTERFACE 00509 // 00510 00511 private void initControlP5() { 00512 00513 controlP5 = new ControlP5(this); 00514 00515 PFont pfont = createFont("Helvetica",10,true); 00516 font = new ControlFont(pfont); 00517 00518 Textarea log = controlP5.addTextarea("Log","", 80,20,400,260); 00519 log.setColorBackground(0xFF000000); 00520 log.setColorForeground(0xFFCCCCCC); 00521 log.valueLabel().setFont(ControlP5.grixel); 00522 log.setLineHeight(16); 00523 log.showScrollbar(); 00524 00525 Textfield input = controlP5.addTextfield("Query", 80,300,400,25); 00526 input.setColorBackground(0xFF000000); 00527 input.valueLabel().setFont(ControlP5.grixel); 00528 00529 //input.setText("What do you know about Cupboard1?"); 00530 00531 input.setAutoClear(true); 00532 input.setFocus(true); 00533 input.keepFocus(true); 00534 00535 00536 checkbox = controlP5.addCheckBox("speech_in",68,360); 00537 checkbox.setColorForeground(color(120)); 00538 checkbox.setColorActive(color(200)); 00539 checkbox.setColorLabel(color(128)); 00540 00541 // add items to a checkbox 00542 checkbox.addItem("Activate Speech Input",23); 00543 checkbox.activate("Activate Speech Input"); 00544 00545 00546 } 00547 00548 void controlEvent(ControlEvent theEvent) { 00549 00550 if( (theEvent.isController()) && 00551 (theEvent.controller() != null) && 00552 (theEvent.controller() instanceof Textfield)) { 00553 handleTextQuery(theEvent.controller().stringValue()); 00554 return; 00555 } 00556 00557 } 00558 00559 00560 public void keyPressed(){ 00561 00562 switch(keyCode) { 00563 case ESC: 00564 this.state = TOP_LEVEL_STATE; 00565 ((Textarea)controlP5.getGroup("Log")).setText(""); 00566 key=0; 00567 break; 00568 } 00569 } 00570 00571 00572 void handleTextQuery(String textquery) { 00573 00574 00575 00576 String log = ((Textarea)controlP5.getGroup("Log")).stringValue(); 00577 log += "Op: "+textquery+"\n"; 00578 00579 String response = answer_query(textquery); 00580 //if(state.equals(TOP_LEVEL_STATE)) { 00581 log += "Robot: "+response+"\n"; 00582 //} 00583 00584 ((Textarea)controlP5.getGroup("Log")).setText(log); 00585 ((Textarea)controlP5.getGroup("Log")).scroll(1); 00586 00587 // also send to tts system 00588 00589 ros.pkg.std_msgs.msg.String m = new ros.pkg.std_msgs.msg.String(); 00590 m.data = response; 00591 mary_pub.publish(m); 00592 00593 System.out.println("out: "+ response); 00594 00595 } 00596 00597 public void showImageInNewWindow(String img) { 00598 00599 int frameWidth = 0; 00600 int frameHeight = 30; 00601 00602 iviewer = new ImageViewerApplet(); 00603 img_window = new JFrame(); 00604 img_window.getContentPane().add(iviewer, BorderLayout.CENTER); 00605 00606 00607 iviewer.init(); 00608 iviewer.setImage(img); 00609 img_window.setSize(iviewer.width + frameWidth, iviewer.height + frameHeight); 00610 00611 img_window.setVisible(true); 00612 img_window.setResizable(false); 00613 00614 } 00615 00616 00620 // 00621 // CLASS UTILITY METHODS 00622 // 00623 00624 00633 private static Class[] getClasses(String packageName) throws ClassNotFoundException, IOException { 00634 ClassLoader classLoader = Thread.currentThread().getContextClassLoader(); 00635 assert classLoader != null; 00636 String path = packageName.replace('.', '/'); 00637 Enumeration<URL> resources = classLoader.getResources(path); 00638 List<File> dirs = new ArrayList<File>(); 00639 while (resources.hasMoreElements()) { 00640 URL resource = resources.nextElement(); 00641 dirs.add(new File(resource.getFile())); 00642 } 00643 ArrayList<Class> classes = new ArrayList<Class>(); 00644 for (File directory : dirs) { 00645 classes.addAll(findClasses(directory, packageName)); 00646 } 00647 return classes.toArray(new Class[classes.size()]); 00648 } 00649 00658 private static List<Class> findClasses(File directory, String packageName) throws ClassNotFoundException { 00659 List<Class> classes = new ArrayList<Class>(); 00660 if (!directory.exists()) { 00661 return classes; 00662 } 00663 File[] files = directory.listFiles(); 00664 for (File file : files) { 00665 if (file.isDirectory()) { 00666 assert !file.getName().contains("."); 00667 classes.addAll(findClasses(file, packageName + "." + file.getName())); 00668 } else if (file.getName().endsWith(".class")) { 00669 classes.add(Class.forName(packageName + '.' + file.getName().substring(0, file.getName().length() - 6))); 00670 } 00671 } 00672 return classes; 00673 } 00674 00680 public static String findRosPackage(String pkgname) { 00681 00682 try { 00683 String line; 00684 Process p = Runtime.getRuntime().exec( "rospack find " + pkgname +""); 00685 BufferedReader pathreader = new BufferedReader(new InputStreamReader(p.getInputStream(), "UTF-8")); 00686 if( (line = pathreader.readLine()) != null) { 00687 return line; 00688 } 00689 } catch (IOException e) { 00690 e.printStackTrace(System.err); 00691 } 00692 return null; 00693 } 00694 00695 00696 00697 00701 // 00702 // LOCAL PROLOG METHODS 00703 // 00704 00705 private static void initProlog() { 00706 try { 00707 Vector<String> args= new Vector<String>(Arrays.asList(jpl.fli.Prolog.get_default_init_args())); 00708 args.add( "-G256M" ); 00709 //args.add( "-q" ); 00710 args.add( "-nosignals" ); 00711 00712 String rosprolog = findRosPackage("rosprolog"); 00713 System.err.println(rosprolog+"/prolog/init.pl"); 00714 jpl.fli.Prolog.set_default_init_args( args.toArray( new String[0] ) ); 00715 00716 // load the appropriate startup file for this context 00717 new jpl.Query("ensure_loaded('"+rosprolog+"/prolog/init.pl"+"')").oneSolution(); 00718 new jpl.Query("register_ros_package('demo_cotesys_fall2010')").oneSolution(); 00719 new jpl.Query("comp_cop:odufinder_listener(_)").oneSolution(); 00720 00721 } catch(Exception e) { 00722 e.printStackTrace(); 00723 } 00724 } 00725 00731 public static HashMap<String, Vector<PrologValue>> executeLocalPrologQuery(String query) { 00732 00733 jpl.Query q = new jpl.Query( "expand_goal(("+query+"),_9), call(_9)" ); 00734 00735 // return null if there are no solutions (empty hash table == true) 00736 if(!q.hasSolution()) { 00737 return null; 00738 } 00739 00740 HashMap<String, Vector<PrologValue>> result = new HashMap<String, Vector<PrologValue>>(); 00741 00742 Hashtable[] solutions = q.allSolutions(); 00743 00744 for(Hashtable solution : q.allSolutions()) { 00745 00746 for(Object key : solution.keySet()) { 00747 00748 jpl.Term t = (jpl.Term) solution.get(key); 00749 00750 if(!result.containsKey(key)) { 00751 result.put(key.toString(), new Vector<PrologValue>()); 00752 } 00753 result.get(key).add(new PrologValue(t.toString())); 00754 } 00755 } 00756 return result; 00757 } 00758 00759 public boolean isLocalProlog() { 00760 return localProlog; 00761 } 00762 00763 00764 public void setLocalProlog(boolean localProlog) { 00765 this.localProlog = localProlog; 00766 } 00767 00768 public static void main(String args[]) { 00769 00770 DialogModule dialog = new DialogModule(); 00771 JFrame window = new JFrame(); 00772 window.add(dialog); 00773 dialog.init(); 00774 00775 window.setSize(550, 440); 00776 window.setVisible(true); 00777 00778 if(args.length>0 && args[0].equals("local")) { 00779 System.err.println("LOCAL PROLOG"); 00780 dialog.setLocalProlog(true); 00781 } 00782 } 00783 } 00784 00785