engagement_generation: engagement_generation/src/edu/wpi/hri/gen/policy/ref/NoPointZone.java Source File

00001 /*
00002  * Software License Agreement (BSD License)
00003  * 
00004  * Copyright (c) 2010, Worcester Polytechnic Institute All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  * * Redistributions of source code must retain the above copyright notice, this
00010  * list of conditions and the following disclaimer. * Redistributions in binary
00011  * form must reproduce the above copyright notice, this list of conditions and
00012  * the following disclaimer in the documentation and/or other materials provided
00013  * with the distribution. * Neither the name of Worcester Polytechnic Institute.
00014  * nor the names of its contributors may be used to endorse or promote products
00015  * derived from 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 package edu.wpi.hri.gen.policy.ref;
00030 
00031 import java.util.ArrayList;
00032 import java.util.List;
00033 
00034 import ros.NodeHandle;
00035 import ros.Ros;
00036 
00037 import edu.wpi.hri.gen.GenerationParams;
00038 import edu.wpi.hri.gen.comm.GazeKnowledge;
00039 import edu.wpi.hri.gen.comm.SituationKnowledge;
00040 import edu.wpi.hri.log.Logger;
00041 import edu.wpi.hri.log.Logger.Colors;
00042 import edu.wpi.hri.log.Logger.LoggerLevel;
00043 
00052 public class NoPointZone {
00053 
00054         private final Logger logger;
00055         private final List<Zone> zones;
00056         private final SituationKnowledge situation;
00057         private final GazeKnowledge gaze;
00058 
00069         public NoPointZone(Logger logger, SituationKnowledge situation,
00070                         GazeKnowledge gaze) {
00071                 this.logger = logger.sub(Colors.SITUATION_KNOWLEDGE, "PT-ZONE");
00072                 this.zones = new ArrayList<NoPointZone.Zone>();
00073                 this.situation = situation;
00074                 this.gaze = gaze;
00075 
00076                 // add in the "you can't point backward" zone
00077                 this.addZone(new Zone(Double.MIN_VALUE, 0, Double.MAX_VALUE,
00078                                 Double.MIN_VALUE));
00079         }
00080 
00088         public boolean addAllZones(NodeHandle handle) {
00089                 try {
00090                         NodeHandle params = Ros.getInstance().createNodeHandle(handle,
00091                                         GenerationParams.NAMESPACE);
00092                         int numZones = params.getIntParam("no_zone/num_zones", false);
00093 
00094                         for (int c = 0; c < numZones; c++) {
00095                                 double x1 = params.getDoubleParam("no_zone/x1_" + c);
00096                                 double y1 = params.getDoubleParam("no_zone/y1_" + c);
00097                                 double x2 = params.getDoubleParam("no_zone/x2_" + c);
00098                                 double y2 = params.getDoubleParam("no_zone/y2_" + c);
00099                                 this.addZone(new Zone(x1, y1, x2, y2));
00100                         }
00101 
00102                 } catch (Exception e) {
00103                         return false;
00104                 }
00105                 return true;
00106         }
00107 
00115         public boolean addZone(Zone z) {
00116                 logger.debug(LoggerLevel.MATH, "Adding new zone: " + z);
00117                 return this.zones.add(z);
00118         }
00119 
00127         public boolean canPointTo(Point pt) {
00128                 if (pt == null || gaze.isLocked())
00129                         return false;
00130 
00131                 pt = situation.convertToRobotSpace(pt);
00132 
00133                 logger.debug(LoggerLevel.MATH, "Checking if robot relative " + pt
00134                                 + " can be pointed to");
00135 
00136                 for (Zone z : this.zones)
00137                         if (z.contains(pt))
00138                                 return false;
00139 
00140                 return true;
00141         }
00142 
00148         public static class Zone {
00149                 private final double xmin;
00150                 private final double ymin;
00151                 private final double xmax;
00152                 private final double ymax;
00153 
00166                 public Zone(double x1, double y1, double x2, double y2) {
00167                         // find the lower and the upper bounds of the rectangle here for
00168                         // ease of use in the contains method
00169                         this.xmin = Math.min(x1, x2);
00170                         this.ymin = Math.min(y1, y2);
00171                         this.xmax = Math.max(x1, x2);
00172                         this.ymax = Math.max(y1, y2);
00173                 }
00174 
00175                 boolean contains(Point pt) {
00176                         return (this.xmin <= pt.getX()) && (this.xmax >= pt.getX())
00177                                         && (this.ymin <= pt.getY()) && (this.ymax >= pt.getY());
00178                 }
00179 
00180                 @Override
00181                 public String toString() {
00182                         return "[xmin:" + xmin + ", ymin:" + ymin + ", xmax:" + xmax
00183                                         + ", ymax:" + ymax + "]";
00184                 }
00185         }
00186 }