Project: engagement_generation License: BSD Dependencies: engagement_msgs engagement_srvs bml_msgs bml_srvs bml_realizer Used by: None All Packages

engagement_generation/src/edu/wpi/hri/gen/comm/BMLEmitListener.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.gen.comm; import java.util.ArrayList; import java.util.List; import ros.NodeHandle; import ros.Publisher; import ros.RosException; import ros.ServiceClient; import ros.Subscriber; import ros.Subscriber.Callback; import ros.pkg.bml_msgs.msg.BMLEmit; import ros.pkg.bml_msgs.msg.Entity; import ros.pkg.bml_msgs.msg.Flag; import ros.pkg.engagement_srvs.srv.RobotDirectedGaze; import ros.pkg.engagement_srvs.srv.RobotMutualFacialGaze; import edu.wpi.hri.bml.behavior.Behavior; import edu.wpi.hri.bml.behavior.EmitBehavior; import edu.wpi.hri.bml.behavior.EmitBehavior.EventBehavior; import edu.wpi.hri.bml.behavior.SyncRef; import edu.wpi.hri.bml.behavior.SyncRef.SyncPoint; import edu.wpi.hri.gen.GenerationParams; import edu.wpi.hri.log.Logger; import edu.wpi.hri.log.Logger.Colors; import edu.wpi.hri.log.Logger.LoggerLevel; public class BMLEmitListener implements Callback<BMLEmit> { private final Logger logger; private int emitCounter; private final List<Thread> threads; private final ServiceClient<RobotDirectedGaze.Request, RobotDirectedGaze.Response, RobotDirectedGaze> dgsrv; private final ServiceClient<RobotMutualFacialGaze.Request, RobotMutualFacialGaze.Response, RobotMutualFacialGaze> mfgsrv; private final Subscriber<BMLEmit> emitSub; private final Publisher<ros.pkg.bml_msgs.msg.Behavior> endEmit; public BMLEmitListener(NodeHandle handle, Logger logger) throws RosException { this.logger = logger.sub(Colors.BML, "EMIT"); this.threads = new ArrayList<Thread>(); this.dgsrv = handle.serviceClient( GenerationParams.ROBOT_DG_PATH.getString(), new RobotDirectedGaze()); this.mfgsrv = handle.serviceClient( GenerationParams.ROBOT_MFG_PATH.getString(), new RobotMutualFacialGaze()); this.emitSub = handle.subscribe( GenerationParams.BML_EMIT_PATH.getString(), new BMLEmit(), this, 100); this.endEmit = handle.advertise("bml/end_emit", new ros.pkg.bml_msgs.msg.Behavior(), 100); this.logger.debug(LoggerLevel.INIT, "Created ..."); } public void shutdown() { this.emitSub.shutdown(); waitForEnd(); this.dgsrv.shutdown(); this.mfgsrv.shutdown(); } public void waitForEnd() { while (threads.size() > 0) { try { threads.get(0).join(); threads.remove(0); } catch (InterruptedException e) { logger.debug(LoggerLevel.ALL, "Failed to wait for end of connection event"); } } } private void robotDG(final ros.pkg.bml_msgs.msg.Behavior behav, final String actor, final String[] objects) { logger.debug(LoggerLevel.POLICY_EXECUTION, "Starting Directed Gaze: " + behav.id); Thread t = new Thread() { public void run() { // create the request RobotDirectedGaze.Request req = new RobotDirectedGaze.Request(); req.actor.id = actor; req.done.value = Flag.NOT_DONE; req.objects = new ArrayList<Entity>(); for (String obj : objects) { Entity ent = new Entity(); ent.id = obj; ent.type = Entity.UNKNOWN; req.objects.add(ent); } try { dgsrv.call(req); logger.debug(LoggerLevel.IO, "finished DG: " + behav.id); endEmit.publish(behav); } catch (RosException e) { logger.printThrowable(LoggerLevel.IO, e); } } }; t.start(); this.threads.add(t); } private void robotMFG(final ros.pkg.bml_msgs.msg.Behavior behav, final String actor) { logger.debug(LoggerLevel.POLICY_EXECUTION, "Starting Mutual Facial Gaze: " + behav.id); Thread t = new Thread() { public void run() { // create the request RobotMutualFacialGaze.Request req = new RobotMutualFacialGaze.Request(); req.actor.id = actor; try { mfgsrv.call(req); logger.debug(LoggerLevel.IO, "finished MFG: " + behav.id); endEmit.publish(behav); } catch (RosException e) { logger.printThrowable(LoggerLevel.IO, e); } } }; t.start(); this.threads.add(t); } @Override public void call(BMLEmit emit) { String[] tokens = emit.body.split(","); if (tokens[0].trim().equalsIgnoreCase("DG")) { String[] objects = new String[tokens.length - 2]; for (int c = 2; c < tokens.length; c++) objects[c - 2] = tokens[c]; this.robotDG(emit.behavior, tokens[1].trim(), objects); } else if (tokens[0].trim().equalsIgnoreCase("MFG")) { this.robotMFG(emit.behavior, tokens[1].trim()); } } public EmitBehavior createMFGEmit(String actor, Behavior source) { String emitID = "mfg-emit-" + this.emitCounter; EmitBehavior emit = new EmitBehavior(logger, emitID, new SyncRef( source, SyncPoint.START, 0.0), new EventBehavior(logger, "mfg-event-" + this.emitCounter, "MFG," + actor), false); this.emitCounter++; return emit; } public EmitBehavior createDGEmit(String actor, String[] targets, Behavior source, boolean required) { String emitID = "dg-emit-" + this.emitCounter; String tars = ""; for (int c = 0; c < targets.length - 1; c++) tars += targets[c] + ","; tars += targets[targets.length - 1]; EmitBehavior emit = new EmitBehavior(logger, emitID, new SyncRef( source, SyncPoint.START, 0.0), new EventBehavior(logger, "dg-event-" + this.emitCounter, "DG," + actor + "," + tars), required); this.emitCounter++; return emit; } }

---

engagement_generation
Author(s): Aaron Holroyd (aholroyd (at) wpi (dot) edu)
autogenerated on Fri Sep 9 10:35:40 2011