Project: engagement_test License: BSD Dependencies: engagement_msgs engagement_srvs Used by: None All Packages

engagement_test/src/edu/wpi/hri/ros/testing/looper/Looper.java Go to the documentation of this file. /* * Software License Agreement (BSD License) * * Copyright (c) 2010, Worcester Polytechnic Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Worcester Polytechnic Institute. nor the names * of its contributors may be used to endorse or promote products * derived from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ package edu.wpi.hri.ros.testing.looper; import java.util.Date; import java.util.Random; import java.util.ArrayList; import ros.NodeHandle; import ros.Ros; import ros.ServiceClient; import ros.Subscriber; import ros.pkg.engagement_msgs.msg.APAction; import ros.pkg.bml_msgs.msg.Behavior; import ros.pkg.bml_msgs.msg.Entity; import ros.pkg.bml_msgs.msg.Flag; import ros.pkg.engagement_msgs.msg.HumanBackchannel; import ros.pkg.engagement_msgs.msg.Performance; import ros.pkg.bml_srvs.srv.BMLGaze; import ros.pkg.bml_srvs.srv.BMLGesture; import ros.pkg.bml_srvs.srv.BMLHead; import ros.pkg.bml_srvs.srv.BMLSpeech; import ros.pkg.engagement_srvs.srv.RobotAdjacencyPair; import ros.pkg.engagement_srvs.srv.RobotDirectedGaze; import ros.pkg.engagement_srvs.srv.RobotMutualFacialGaze; import edu.wpi.hri.ros.testing.lib.Logger; import edu.wpi.hri.ros.testing.lib.LooperParams; import edu.wpi.hri.ros.testing.lib.TestingParams; public class Looper { Random rand_ = new Random(new Date().getTime()); public static Logger logger_; private Thread cycle_thread_; private NodeHandle handle_; private static int DIRECTED_GAZE = 0; private static int MUTUAL_FACIAL_GAZE = 1; private static int ADJACENCY_PAIR = 2; private static int BACKCHANNEL = 3; private static int INVALID_SELECTION = -1; private String recognize_; private String point_to_; private String actor_; private boolean wait_for_backchannel_ = false; private static final String speeches_[] = { "Four score and seven years ago our fathers brought forth on this continent, a new nation, conceived in Liberty, and dedicated to the proposition that all men are created equal.", "I have a dream that one day this nation will rise up and live out the true meaning of its creed: We hold these truths to be self-evident, that all men are created equal.", "We choose to go to the moon. We choose to go to the moon in this decade and do the other things, not because they are easy, but because they are hard, because that goal will serve to organize and measure the best of our energies and skills, because that challenge is one that we are willing to accept, one we are unwilling to postpone, and one which we intend to win, and the others, too. ", "So, first of all, let me assert my firm belief that the only thing we have to fear is fear itself -- nameless, unreasoning, unjustified terror which paralyzes needed efforts to convert retreat into advance. In every dark hour of our national life, a leadership of frankness and of vigor has met with that understanding and support of the people themselves which is essential to victory.", "One, a robot may not injure a human being, or through inaction, allow a human being to come to harm; Two, a robot must obey the orders given it by human beings except where such orders would conflict with the First Law; Three, a robot must protect its own existence as long as such protection does not conflict with the First or Second Laws.", "Kipple is useless objects, like junk mail or match folders after you use the last match or gum wrappers of yesterday's homeopape. When nobody's around, kipple reproduces itself. For instance, if you go to bed leaving any kipplpe around your apartment, when you wake up next morning there's twice as much of it. It always gets more and more.", "The first ten million years were the worst, and the second ten million, they were the worst too. The third ten million I didn't enjoy at all. After that I went into a bit of a decline.", "It is indeed true that at times like this R2 and I wish we were more than mechanical beings and we were really alive so we could share your feelings with you." }; private int last_selected_speech_ = -1; ServiceClient<BMLGesture.Request, BMLGesture.Response, BMLGesture> ctl_point_srv_; ServiceClient<BMLGaze.Request, BMLGaze.Response, BMLGaze> ctl_track_srv_; ServiceClient<BMLHead.Request, BMLHead.Response, BMLHead> ctl_head_srv_; ServiceClient<BMLSpeech.Request, BMLSpeech.Response, BMLSpeech> ctl_say_srv_; ServiceClient<RobotDirectedGaze.Request, RobotDirectedGaze.Response, RobotDirectedGaze> rec_dg_srv_; ServiceClient<RobotMutualFacialGaze.Request, RobotMutualFacialGaze.Response, RobotMutualFacialGaze> rec_mfg_srv_; ServiceClient<RobotAdjacencyPair.Request, RobotAdjacencyPair.Response, RobotAdjacencyPair> rec_ap_srv_; private Subscriber.QueueingCallback<HumanBackchannel> human_bc_q_; private Subscriber.QueueingCallback<Performance> pernod_q; @SuppressWarnings("unused") private Subscriber<HumanBackchannel> human_bc_s_; @SuppressWarnings("unused") private Subscriber<Performance> pernod_s; public void loadParameters() { // Create a private configuration node handle NodeHandle priv_ = Ros.getInstance().createNodeHandle( "~" + TestingParams.CONF_NAMESPACE.toString()); try { // Create the actor param in the configuration namespace String actor_param = LooperParams.ACTOR_PARAM.toString(); // Grab the parameter from the configuration namespace this.actor_ = priv_.getStringParam(actor_param, false); } catch (Exception e) { logger_.fatal(Logger.NO_AREA, "Unable to determine which " + "actor to use."); } try { // Create the recognize param in the configuration namespace String recognize_param = LooperParams.RECOGNIZE_PARAM.toString(); // Grab the parameters from the configuration namespace this.recognize_ = priv_.getStringParam(recognize_param, false); } catch (Exception e) { logger_.fatal(Logger.NO_AREA, "Unable to determine which " + "behavior to recognize."); } // Determine if the selection was valid int selection = this.whichBehavior(this.recognize_); // If we're recognizing directed gaze determine // which object to point to if (selection == 0) { // Create the point to param in the configuration namespace String point_to_param = LooperParams.POINT_TO_PARAM.toString(); try { // Grab the object to point to from the configuration namespace this.point_to_ = priv_.getStringParam(point_to_param, false); } catch (Exception e) { logger_.fatal(Logger.NO_AREA, "Unable to load object to" + "point to"); } } else if (selection == -1) { logger_.fatal(Logger.NO_AREA, "Attempting to recognize invalid " + "behavior [" + this.recognize_ + "]"); } } public Looper(NodeHandle handle) { this.handle_ = handle; // Create the logger using the conf node handle Looper.logger_ = new Logger("LOOPER", Ros.getInstance() .createNodeHandle("~" + TestingParams.CONF_NAMESPACE)); // Load the parameters from the param server this.loadParameters(); try { // Create strings for each of the namespaces String control_ns = TestingParams.CONTROL_NAMESPACE.toString(); String recog_ns = TestingParams.RECOGNITION_NAMESPACE.toString(); String robot_ns = recog_ns + "/" + TestingParams.ROBOT_NAMESPACE.toString(); String human_ns = recog_ns + "/" + TestingParams.HUMAN_NAMESPACE.toString(); String performing_ns = TestingParams.PERFORMING_NAMESPACE .toString(); // Create service clients for all the control topics this.ctl_point_srv_ = handle.serviceClient(control_ns + "/" + TestingParams.CTL_POINT_TOPIC.toString(), new BMLGesture(), false); this.ctl_track_srv_ = handle.serviceClient(control_ns + "/" + TestingParams.CTL_TRACK_TOPIC.toString(), new BMLGaze(), false); this.ctl_head_srv_ = handle.serviceClient(control_ns + "/" + TestingParams.CTL_HEAD_TOPIC.toString(), new BMLHead(), false); this.ctl_say_srv_ = handle.serviceClient(control_ns + "/" + TestingParams.CTL_SAY_TOPIC.toString(), new BMLSpeech(), false); // Create service clients for all the recognition topics this.rec_dg_srv_ = handle.serviceClient(robot_ns + "/" + TestingParams.REC_DIRECTED_GAZE_TOPIC.toString(), new RobotDirectedGaze(), false); this.rec_mfg_srv_ = handle.serviceClient(robot_ns + "/" + TestingParams.REC_MUTUAL_FACIAL_GAZE_TOPIC.toString(), new RobotMutualFacialGaze(), false); this.rec_ap_srv_ = handle.serviceClient(robot_ns + "/" + TestingParams.REC_ADJACENCY_PAIR_TOPIC.toString(), new RobotAdjacencyPair(), false); this.human_bc_q_ = new Subscriber.QueueingCallback<HumanBackchannel>(); this.human_bc_s_ = handle.subscribe(human_ns + "/" + TestingParams.REC_BACKCHANNEL_TOPIC.toString(), new HumanBackchannel(), this.human_bc_q_, 100); this.pernod_q = new Subscriber.QueueingCallback<Performance>(); this.pernod_s = handle.subscribe(performing_ns + "/" + TestingParams.PER_NOD_TOPIC.toString(), new Performance(), this.pernod_q, 100); } catch (Exception e) { e.printStackTrace(); } // create a Thread for handling the ROS stuff cycle_thread_ = new Thread() { public void run() { try { // Sleep for five seconds at startup. Thread.sleep(5000); } catch (Exception e) { e.printStackTrace(); } while (isValid()) { try { // Determine which behavior to perform int selection = whichBehavior(recognize_); if (selection == DIRECTED_GAZE) performDirectedGaze(); else if (selection == MUTUAL_FACIAL_GAZE) performMutualFacialGaze(); else if (selection == ADJACENCY_PAIR) performAdjacencyPair(); else if (selection == BACKCHANNEL) { performBackchannel(); } else logger_.error(Logger.NO_AREA, "Unrecognized recognition " + "selection being used."); // Sleep for 2.5 seconds Thread.sleep(2500); // Create a new track message BMLGaze.Request track_msg = new BMLGaze.Request(); track_msg.target.type = Entity.QUADRANT; // Grab the value of the upper right quadrant from // the vision position message track_msg.target.id = "UP_RIGHT"; // Instruct the robot to look off in a // different direction (up and to the right) ctl_track_srv_.call(track_msg); // Sleep for another 2.5 seconds Thread.sleep(2500); } catch (Exception e) { e.printStackTrace(); } } } }; cycle_thread_.start(); } public synchronized boolean isValid() { return this.handle_.isValid(); } public void run() { // Loop until the program is terminated while (this.handle_.isValid()) { try { // Spin to handle input this.handle_.spinOnce(); // Handle incoming backchannel events this.checkBackchannel(); Thread.sleep(10); } catch (Exception e) { e.printStackTrace(); } } } private ArrayList<Entity> getEntitiesFromObjects(String objects[]) { // Create an array of entities of the same size ArrayList<Entity> entities = new ArrayList<Entity>(); // Traverse through all the objects for (String obj : objects) { // Create the new entity object in the array Entity ent = new Entity(); ent.id = obj; ent.type = Entity.OBJECT; entities.add(ent); } return entities; } public void performAdjacencyPair() { // ADJACENCY PAIR CASE. logger_.debug(Logger.NO_AREA, "Performing an adjacency pair with " + "actor [" + this.actor_ + "]."); // Create the speech text without any markup String please = "<mark name=\"track FACE " + this.actor_ + "\" />Please respond"; // Speak the text this.speak(please); // Create the message for adjacency pair RobotAdjacencyPair.Request ap_msg = new RobotAdjacencyPair.Request(); ap_msg.actor.id = this.actor_; try { // Call the recognize adjacency pair service RobotAdjacencyPair.Response ap_response = this.rec_ap_srv_ .call(ap_msg); // Determine if the service succeeded or failed if (ap_response.result.value == Flag.SUCCESS) { // Determine which action was used to complete the AP switch (ap_response.action.value) { case APAction.NOD: this.speak("You successfully nodded your head."); break; case APAction.SHAKE: this.speak("You successfully shook your head."); break; case APAction.POINT: this.speak("You successfully pointed."); break; case APAction.LOOK: this.speak("You successfully looked."); break; case APAction.SPEECH: this.speak("You successfully spoke."); break; case APAction.EXTENDED_ACTION: this.speak("You successfully performed an action."); break; default: this.speak("Success."); break; } } else if (ap_response.result.value == Flag.FAILURE) this.speak("You failed to respond."); else logger_.error(Logger.NO_AREA, "Invalid arguments to " + "adjacency pair service."); } catch (Exception e) { logger_.warn(Logger.NO_AREA, "Unable to successfully call " + "the adjacency pair service."); e.printStackTrace(); } } public void performBackchannel() { // BACKCHANNEL CASE logger_.debug(Logger.NO_AREA, "Performing a long speech to " + "ellicit backchannels from actor [" + this.actor_ + "]."); // Create the random speech to use String speech = this.selectRandomSpeech(); // Wait for backchannels to occur this.wait_for_backchannel_ = true; // Speak the text, have the robot track the human's face throughout this.speak("<mark name=\"track FACE " + this.actor_ + "\" />" + speech); // Stop waiting for backchannel to occur this.wait_for_backchannel_ = false; } public void performDirectedGaze() { // THE DIRECTED GAZE CASE logger_.debug(Logger.NO_AREA, "Performing a directed gaze to [" + this.point_to_ + "] with actor [" + this.actor_ + "]."); // Create the speech text with markup to point and // look at the blue plate // also put a markup at the end to look back at the human String look_here = "<mark name=\"track OBJECT " + this.point_to_ + "\" />Please look <mark name=\"point EITHER " + this.point_to_ + "\" />here"; // Create the array to hold the object to point to String objects[] = new String[1]; objects[0] = this.point_to_; // Speak the text this.speak(look_here); // Create the message for directed gaze RobotDirectedGaze.Request dg_msg = new RobotDirectedGaze.Request(); dg_msg.actor.id = "face0"; dg_msg.done.value = Flag.NOT_DONE; dg_msg.objects = this.getEntitiesFromObjects(objects); try { // Call the recognize directed gaze service RobotDirectedGaze.Response dg_response = this.rec_dg_srv_ .call(dg_msg); // Determine if the service succeeded or failed if (dg_response.result.value == Flag.SUCCESS) this.speak("You successfully looked."); else if (dg_response.result.value == Flag.FAILURE) this.speak("You failed to look."); else logger_.error(Logger.NO_AREA, "Invalid arguments to " + "directed gaze service."); } catch (Exception e) { logger_.warn(Logger.NO_AREA, "Unable to successfully call " + "the directed gaze service."); e.printStackTrace(); } try { // Create the point message to end the pointing BMLGesture.Request pt_msg = new BMLGesture.Request(); pt_msg.side.value = Flag.EITHER; pt_msg.behav.synchPoint = Behavior.END; // Call the point service to end ctl_point_srv_.call(pt_msg); } catch (Exception e) { logger_.warn(Logger.NO_AREA, "Unable to successfully call " + "the end control point service."); e.printStackTrace(); } } public void performMutualFacialGaze() { // MUTUAL FACIAL GAZE CASE. logger_.debug(Logger.NO_AREA, "Performing a mutual facial gaze " + "with actor [" + this.actor_ + "]."); // Create the speech text with markup to look at the actor's face String look_at_me = "<mark name=\"track FACE " + this.actor_ + "\" />Please look at me"; // Speak the text this.speak(look_at_me); // Create the message for mutual facial gaze using the actor RobotMutualFacialGaze.Request mfg_msg = new RobotMutualFacialGaze.Request(); mfg_msg.actor.id = this.actor_; try { // Call the recognize mutual facial gaze service RobotMutualFacialGaze.Response mfg_response = this.rec_mfg_srv_ .call(mfg_msg); if (mfg_response.result.value == Flag.SUCCESS) this.speak("You successfully looked."); else if (mfg_response.result.value == Flag.FAILURE) this.speak("You failed to look at me."); else logger_.error(Logger.NO_AREA, "Invalid arguments to " + "mutual facial gaze service."); } catch (Exception e) { logger_.error(Logger.NO_AREA, "Unable to successfully call " + "the mutual facial gaze service."); e.printStackTrace(); } } public void checkBackchannel() { // check for shakes while (!this.human_bc_q_.isEmpty()) { try { HumanBackchannel msg = this.human_bc_q_.pop(); // Make sure this was the actor's backchannel, and that // the robot is waiting for a backchannel to occur if (this.wait_for_backchannel_ && msg.actor.id.equals(this.actor_)) { logger_.info(Logger.NO_AREA, "Received a human " + "backchannel message"); // The actor performed a backchannel event, respond // by having the robot nod BMLHead.Request nod_msg = new BMLHead.Request(); nod_msg.motion = BMLHead.Request.NOD; nod_msg.amount = BMLHead.Request.SMALL; nod_msg.behav.id = "nod"; nod_msg.behav.synchPoint = Behavior.START; // Call the nod message this.ctl_head_srv_.call(nod_msg); } } catch (Exception e) { e.printStackTrace(); } } } public void speak(String text) { // Make sure we have a valid service to call, and text to speak if (this.ctl_say_srv_ != null && text.length() > 0) { // Create the control say request message BMLSpeech.Request msg = new BMLSpeech.Request(); // Configure the message to say the given text msg.behav.synchPoint = Behavior.START; msg.behav.id = "looper-say"; msg.text = text; // Call the service with the given message try { logger_.debug(Logger.NO_AREA, "Calling Say service."); // Call the say service and keep track of the response BMLSpeech.Response response = this.ctl_say_srv_.call(msg); // Determine if the say service was called successfully if (response.status.value == Flag.TRUE) logger_.debug(Logger.NO_AREA, "Called Say service [" + text + "]"); else logger_.error(Logger.NO_AREA, "Failed to call Say service [" + text + "]"); msg.behav.synchPoint = Behavior.END; this.ctl_say_srv_.call(msg); } catch (Exception e) { e.printStackTrace(); } } } public int whichBehavior(String selection) { // Transform the selection to all lowercase selection = selection.toLowerCase(); // Determine which goal was selected if (selection.equals("directed_gaze") || selection.equals("directedgaze") || selection.equals("directed gaze") || selection.equals("dg") || selection.equals("dirgaze") || selection.equals("dir gaze") || selection.equals("dir_gaze") || selection.equals("directed")) { return DIRECTED_GAZE; } else if (selection.equals("mutual_facial_gaze") || selection.equals("mutual") || selection.equals("mf gaze") || selection.equals("mutualfacialgaze") || selection.equals("mfg") || selection.equals("mutual facial gaze") || selection.equals("mfgaze") || selection.equals("mf gaze") || selection.equals("mf_gaze")) { return MUTUAL_FACIAL_GAZE; } else if (selection.equals("adjacency_pair") || selection.equals("ap") || selection.equals("adjacencypair") || selection.equals("adj_pair") || selection.equals("adjacency pair") || selection.equals("adj pair") || selection.equals("adjacency")) { return ADJACENCY_PAIR; } else if (selection.equals("backchannel") || selection.equals("bc") || selection.equals("back_channel") || selection.equals("bchannel") || selection.equals("b_channel") || selection.equals("back_chan") || selection.equals("backchan")) { return BACKCHANNEL; } else return INVALID_SELECTION; } public String selectRandomSpeech() { int index = -1; do { // Get a random speech, // different from the one previously selected index = this.rand_.nextInt(Looper.speeches_.length); } while (index == this.last_selected_speech_); // Store the selected speech for next time this.last_selected_speech_ = index; // Return the randomly selected speech return Looper.speeches_[index]; } public static void main(String[] args) { // Initialize ROS Ros.getInstance().init("looper"); // Create a default node handle NodeHandle handle = Ros.getInstance().createNodeHandle(); // Create an instance of the Looper object Looper looper = new Looper(handle); // Run the Echo node looper.run(); } }

---

engagement_test
Author(s): Aaron Holroyd (aholroyd (at) wpi (dot) edu), Brett Ponsler (bponsler (at) wpi (dot) edu)
autogenerated on Fri Sep 9 10:35:37 2011