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/SituationKnowledge.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.Hashtable; import java.util.Set; import ros.NodeHandle; import ros.RosException; import ros.Subscriber; import ros.pkg.bml_msgs.msg.Entity; import ros.pkg.bml_msgs.msg.EntityPosition; import edu.wpi.hri.gen.GenerationParams; import edu.wpi.hri.gen.policy.ref.Point; import edu.wpi.hri.log.Logger; import edu.wpi.hri.log.Logger.Colors; import edu.wpi.hri.log.Logger.LoggerLevel; public class SituationKnowledge implements Subscriber.Callback<EntityPosition> { private final Logger logger; private final Hashtable<String, Point> locations; private final Subscriber<EntityPosition> posSub; private final Hashtable<String, Point> objects; public SituationKnowledge(Logger logger, NodeHandle handle) throws RosException { this.logger = logger.sub(Colors.SITUATION_KNOWLEDGE, "WORLD"); this.locations = new Hashtable<String, Point>(); this.objects = new Hashtable<String, Point>(); this.posSub = handle.subscribe("vision/entity_position", new EntityPosition(), this, 100); this.logger.debug(LoggerLevel.INIT, "setup ..."); } public synchronized void shutdown() { this.posSub.shutdown(); } @Override public synchronized void call(EntityPosition pos) { logger.debug(LoggerLevel.IO, "Received new [" + pos.entity.id + "] location (" + pos.x + ", " + pos.y + ", " + pos.z + ") at rotation (" + pos.rotation + ")"); Point loc = new Point(pos.x, pos.y, pos.z, pos.rotation); this.locations.put(pos.entity.id, loc); //TODO: possibly remove the OBJECT check after moving coordinate transforms if (pos.entity.type == Entity.OBJECT) { this.objects.put(pos.entity.id, loc); } } public synchronized Set<String> getAllObjects() { return this.objects.keySet(); } public synchronized Point get(String id) { if (id == null) throw new InvalidIDException("ID can not be null"); else { Point val = this.locations.get(id); if (val == null) throw new InvalidIDException("ID could not be found: " + id); return val; } } public synchronized Point getRobot() { logger.debug(LoggerLevel.ALL, "retrieving robot position"); return this.get("ROBOT"); } public synchronized Point getRobotHead() { Point robot = getRobot(); logger.debug(LoggerLevel.ALL, "retrieving robot head position"); return new Point(robot.getX(), robot.getY(), GenerationParams.ROBOT_HEAD_HEIGHT.getDouble(), robot.getzRot()); } public synchronized Point getRobotLeftHand() { return this.get("ROBOT_LEFT_HAND"); } public Point getRobotRightHand() { return this.get("ROBOT_RIGHT_HAND"); } public synchronized Point getRobotLeftShoulder() { Point robot = this.getRobot(); double shoulderLength = GenerationParams.ROBOT_SHOULDER_LENGTH .getDouble(); double x = robot.getX() - (Math.cos(Math.PI * robot.getzRot() / 180.0) * shoulderLength); double y = robot.getY() - (Math.sin(Math.PI * robot.getzRot() / 180.0) * shoulderLength); double z = GenerationParams.ROBOT_SHOULDER_HEIGHT.getDouble(); return new Point(x, y, z); } public synchronized Point getRobotRightShoulder() { Point robot = this.getRobot(); double shoulderLength = GenerationParams.ROBOT_SHOULDER_LENGTH .getDouble(); double x = robot.getX() + (Math.cos(Math.PI * robot.getzRot() / 180.0) * shoulderLength); double y = robot.getY() + (Math.sin(Math.PI * robot.getzRot() / 180.0) * shoulderLength); double z = GenerationParams.ROBOT_SHOULDER_HEIGHT.getDouble(); return new Point(x, y, z); } public synchronized Point convertToRobotSpace(Point from) { Point robot = this.getRobot(); logger.debug(LoggerLevel.MATH, "CONVERTING $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ "); logger.debug(LoggerLevel.MATH, "\tRobot position: " + robot.toString()); logger.debug(LoggerLevel.MATH, "\tfrom position: " + from.toString()); double fromDist = from.xyDistance(robot); logger.debug(LoggerLevel.MATH, "\tdistance: " + fromDist); double angle = Math.atan2(from.getY() - robot.getY(), from.getX() - robot.getX()); logger.debug(LoggerLevel.MATH, "\tangle: " + angle); logger.debug(LoggerLevel.MATH, "\trobot rotation: " + robot.getzRot()); angle = ((Math.PI / 2) - angle) + (robot.getzRot() * Math.PI / 180.0); logger.debug(LoggerLevel.MATH, "\tOffset angle: " + angle); double x = Math.sin(angle) * fromDist; double y = Math.cos(angle) * fromDist; double z = from.getZ(); logger.debug(LoggerLevel.MATH, "DONE $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ "); return new Point(x, y, z); } public synchronized boolean isObjectOnRight(String object) { // grab the useful information Point robot = getRobot(); Point loc = get(object); double theta = robot.getzRot() * Math.PI / 180.0; double x = ((loc.getX() - robot.getX()) * Math.cos(theta)) + (loc.getY() * Math.sin(theta)); // then finally check if the loc is to the right of the robot return x > 0; } @SuppressWarnings("serial") public static class InvalidIDException extends RuntimeException { public InvalidIDException(String str) { super(str); } } }

---

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