SpatialCalculator.java
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011, 2012
00004  *
00005  * School of Engineering, Cardiff University, UK
00006  *
00007  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00008  *
00009  * Project name: srs EU FP7 (www.srs-project.eu)
00010  * ROS stack name: srs
00011  * ROS package name: srs_knowledge
00012  * Description: 
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *
00016  * @author Ze Ji, email: jiz1(at)cf.ac.uk
00017  *
00018  * Date of creation: Apr 2012
00019  * ToDo: 
00020  *
00021  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *       * Redistributions of source code must retain the above copyright
00027  *         notice, this list of conditions and the following disclaimer.
00028  *       * Redistributions in binary form must reproduce the above copyright
00029  *         notice, this list of conditions and the following disclaimer in the
00030  *         documentation and/or other materials provided with the distribution.
00031  *       * Neither the name of the school of engineering, Cardiff University
00032  *         nor the names of its contributors may be used to endorse or promote 
00033  *         products derived from this software without specific prior written 
00034  *         permission.
00035  *
00036  * This program is free software: you can redistribute it and/or modify
00037  * it under the terms of the GNU Lesser General Public License LGPL as 
00038  * published by the Free Software Foundation, either version 3 of the 
00039  * License, or (at your option) any later version.
00040  * 
00041  * This program is distributed in the hope that it will be useful,
00042  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00043  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00044  * GNU Lesser General Public License LGPL for more details.
00045  * 
00046  * You should have received a copy of the GNU Lesser General Public 
00047  * License LGPL along with this program. 
00048  * If not, see <http://www.gnu.org/licenses/>.
00049  *
00050  ****************************************************************/
00051 
00052 package org.srs.srs_knowledge.utils;
00053 
00054 import java.io.*;
00055 import java.util.StringTokenizer;
00056 import java.util.ArrayList;
00057 import ros.pkg.srs_knowledge.msg.*;
00058 import ros.pkg.srs_knowledge.srv.*;
00059 import ros.pkg.geometry_msgs.msg.Pose2D;
00060 import org.srs.srs_knowledge.knowledge_engine.*;
00061 import ros.pkg.srs_msgs.msg.SRSSpatialInfo;
00062 
00063 import tfjava.*;
00064 
00065 import javax.vecmath.Quat4d;
00066 import javax.vecmath.Vector3d;
00067 import javax.vecmath.Point3d;
00068 import javax.vecmath.Matrix4d;
00069 
00070 import math.geom2d.*;
00071 import math.geom2d.line.LineSegment2D;
00072 import math.geom2d.polygon.Polygon2DUtils;
00073 import math.geom2d.polygon.Polygon2D;
00074 import math.geom2d.polygon.SimplePolygon2D;
00075 
00076 import math.geom2d.Point2D;
00077 import math.geom3d.Point3D;
00078 
00079 public class SpatialCalculator 
00080 {
00081     public SpatialCalculator() {
00082     }
00083 
00084     public static void testTF() 
00085     {
00086         System.out.println(" +++++++++++++++++++HELLO TEST TF+++++++++++++++++++");
00087 
00088         Vector3d orig = new Vector3d(0,0,0);
00089         Quat4d q4d = new Quat4d(0.00000000e+00, 0.00000000e+00, 9.99999683e-01, 7.96326711e-04);
00090         ros.pkg.geometry_msgs.msg.Point point = new ros.pkg.geometry_msgs.msg.Point();
00091         point.x = 1;
00092         point.y = -1;
00093         point.z = 1;
00094         ros.pkg.geometry_msgs.msg.Point np = SpatialCalculator.transformPoint(orig, q4d, point);
00095         System.out.println("NEW POINT: " + np.x + "  " + np.y + "  " + np.z);
00096 
00097         System.out.println(" +++++++++++++++++++++++++++++++++++++++++++++++++++ ");
00098         
00099         SRSSpatialInfo o1 = new SRSSpatialInfo();
00100         o1.l = 5;
00101         o1.w = 5;
00102         o1.h = 5;
00103 
00104         o1.pose.position.x = 3;
00105         o1.pose.position.y = 3;
00106         o1.pose.position.z = 10;
00107         o1.pose.orientation.w = 1;
00108 
00109         SRSSpatialInfo o2 = new SRSSpatialInfo();
00110         o2.l = 10;
00111         o2.w = 10;
00112         o2.h = 10;
00113         o2.pose.position.z = 0;
00114         o2.pose.orientation.w = 1;
00115         if(SpatialCalculator.ifOnObject(o1, o2, 1)) {
00116             System.out.println(" YES. O1 is on O2");
00117         }
00118         else {
00119             System.out.println(" NO. O1 is NOT on O2");
00120         }
00121 
00122         System.out.println(" +++++++++++++++++++++++++++++++++++++++++++++++++++ ");
00123         BoundingBoxDim bbd = InformationRetrieval.retrieveBoundingBoxInfo(OntoQueryUtil.GlobalNameSpace + "Milkbox");
00124         System.out.println("Retrieved DIm: " + bbd.l + "  " + bbd.w + "  " + bbd.h);
00125         System.out.println(" +++++++++++++++++++++++++++++++++++++++++++++++++++ ");
00126     }
00127 
00128 
00129     public static ros.pkg.geometry_msgs.msg.Point transformPoint(Vector3d translation, Quat4d rotation, ros.pkg.geometry_msgs.msg.Point point) {
00130         
00131         tfjava.StampedTransform transform = new tfjava.StampedTransform(translation, rotation, null, "", "");
00132         Point3d p = new Point3d(point.x, point.y, point.z);
00133         Point3d newp = new Point3d();
00134         transform.transformPoint(p, newp);
00135         
00136         ros.pkg.geometry_msgs.msg.Point ret = new ros.pkg.geometry_msgs.msg.Point();
00137         ret.x = newp.x;
00138         ret.y = newp.y;
00139         ret.z = newp.z;
00140         return ret;
00141     }
00142 
00143     public static Point3d transformPoint(StampedTransform transform, Point3d point) {
00144         Point3d ret = new Point3d();
00145         transform.transformPoint(point, ret);
00146         return ret;
00147     }
00148 
00149     public static boolean ifOverlapping(Polygon2D p1, Polygon2D p2) {
00150         Polygon2D overlap = Polygon2DUtils.intersection(p1, p2);
00151         return overlap.getArea() > 0;
00152     }
00153     
00157     public static boolean ifOnObject(SRSSpatialInfo obj1, SRSSpatialInfo obj2, int poseCfg) {
00158         // error checking ..
00159 
00160         ArrayList<ros.pkg.geometry_msgs.msg.Point> corners1 = getBoundingBoxTopCorners(obj1);
00161         ArrayList<ros.pkg.geometry_msgs.msg.Point> corners2 = getBoundingBoxTopCorners(obj2);
00162         if(ifOverlapping(createPolygon(corners1), createPolygon(corners2))) {
00163             //System.out.println("YES.... OVerlapping..");
00164         }
00165         else{
00166             //System.out.println("NOOO.... OVerlapping..");
00167         }
00168         switch(poseCfg) {
00169         case 0:
00170             return ifOverlapping(createPolygon(corners1), createPolygon(corners2)) && Math.abs(obj1.pose.position.z - obj2.h + obj2.pose.position.z) <= obj1.h/2;
00171         case 1:
00172             return ifOverlapping(createPolygon(corners1), createPolygon(corners2)) && Math.abs(obj1.pose.position.z - obj1.h/2 - obj2.h + obj2.pose.position.z - obj2.h/2) <= obj1.h/2;
00173         default:
00174             return ifOverlapping(createPolygon(corners1), createPolygon(corners2)) && obj2.pose.position.z <= obj1.pose.position.z;
00175         }
00176     }
00177     
00178     private static Polygon2D createPolygon(ArrayList<ros.pkg.geometry_msgs.msg.Point> vertex) {
00179         SimplePolygon2D p = new SimplePolygon2D();
00180         for (ros.pkg.geometry_msgs.msg.Point v : vertex) {
00181             Point2D p2d = new Point2D(v.x, v.y);
00182             p.addVertex(p2d);
00183         }
00184         
00185         return p;
00186     }
00187 
00188     private static ArrayList<ros.pkg.geometry_msgs.msg.Point> getBoundingBoxTopCorners(SRSSpatialInfo obj) {
00189         
00190         ArrayList<ros.pkg.geometry_msgs.msg.Point> corners = new ArrayList<ros.pkg.geometry_msgs.msg.Point>();
00191         Vector3d orig = new Vector3d(obj.pose.position.x, obj.pose.position.y, obj.pose.position.z);
00192         Quat4d q4d = new Quat4d(obj.pose.orientation.x, obj.pose.orientation.y, obj.pose.orientation.x, obj.pose.orientation.w);
00193         ros.pkg.geometry_msgs.msg.Point point = new ros.pkg.geometry_msgs.msg.Point();
00194         point.x = -obj.l/2;
00195         point.y = -obj.w/2;
00196         point.z = obj.h;
00197         ros.pkg.geometry_msgs.msg.Point np = SpatialCalculator.transformPoint(orig, q4d, point);
00198         corners.add(np);
00199 
00200         point.x = obj.l/2;
00201         point.y = -obj.w/2;
00202         point.z = obj.h;
00203         np = SpatialCalculator.transformPoint(orig, q4d, point);
00204         corners.add(np);
00205 
00206         point.x = obj.l/2;
00207         point.y = obj.w/2;
00208         point.z = obj.h;
00209         np = SpatialCalculator.transformPoint(orig, q4d, point);
00210         corners.add(np);
00211 
00212         point.x = -obj.l/2;
00213         point.y = obj.w/2;
00214         point.z = obj.h;
00215         np = SpatialCalculator.transformPoint(orig, q4d, point);
00216         corners.add(np);
00217 
00218         return corners;
00219     }
00220 
00221     // simply use awt Polygon.contains() 
00222     //private static boolean pointInPolygon(double x, double y, Point2D[] ps) {
00223         
00224     //  return false;
00225     // }
00226 
00227     public static String nearestObject(ros.pkg.geometry_msgs.msg.Pose pose, String objectTypeURI) {
00228         //GetObjectsOnMap.Response resObj = OntoQueryUtil.getObjectsOnMap(OntoQueryUtil.MapName, true);
00229         
00230         //Iterator<Individual> instancesOfObject = KnowledgeEngine.ontoDB.getInstancesOfClass(className);
00231         
00232         /*
00233           double minDis = 0;
00234         String res = "";
00235         for (int i = 0; i < resObj.objects.size(); i++) {
00236             double temp = SpatialCalculator.distanceBetween(pose, resObj.objectsInfo.get(i).pose);
00237 
00238             if(temp <= minDis) {
00239                 minDis = temp;
00240                 res = resObj.objects.get(i);
00241             }
00242         }
00243         return res;
00244         */
00245         GetObjectsOnMap.Response re = new GetObjectsOnMap.Response();
00246         re = OntoQueryUtil.getObjectsOnMapOfType(objectTypeURI, true);
00247         double minDis = 0;
00248         String res = "";
00249         if(re.objects.size() > 0) {
00250             minDis = SpatialCalculator.distanceBetween(pose, re.objectsInfo.get(0).pose);
00251             res = re.objects.get(0);
00252         }
00253         else {
00254             return "";
00255         }
00256 
00257         for(int i = 1; i < re.objects.size(); i++) {
00258             double temp = SpatialCalculator.distanceBetween(pose, re.objectsInfo.get(i).pose);
00259             
00260             if(temp <= minDis) {
00261                 minDis = temp;
00262                 res = re.objects.get(i);
00263             }
00264         }
00265         return OntoQueryUtil.ObjectNameSpace + res;
00266     } 
00267 
00268     private static double distanceBetween(ros.pkg.geometry_msgs.msg.Pose obj1, ros.pkg.geometry_msgs.msg.Pose obj2) {
00269         
00270         math.geom3d.Point3D p1 = new math.geom3d.Point3D(obj1.position.x, obj1.position.y, obj1.position.z);
00271         math.geom3d.Point3D p2 = new math.geom3d.Point3D(obj2.position.x, obj2.position.y, obj2.position.z);
00272         return p1.getDistance(p2);
00273     }
00274 
00278     public static String workspaceHolding(SRSSpatialInfo spaInfo) {
00279 
00280         GetWorkspaceOnMap.Response resWS = OntoQueryUtil.getWorkspaceOnMap(OntoQueryUtil.MapName, true);
00281         // pair-wise comparison --- if condition met, then update the knowledge base
00282 
00283         for (int j = 0; j < resWS.objectsInfo.size(); j++) {
00284             if(SpatialCalculator.ifOnObject(spaInfo, resWS.objectsInfo.get(j), -1)) {
00285                 return OntoQueryUtil.ObjectNameSpace + resWS.objects.get(j);
00286             }
00287         }
00288 
00289         //if (j == resWS.objectsInfo.size()) {
00290         //System.out.println("NO MATCH.... found for : [" + spaInfo.l + "  " + spaInfo.h + "  " + spaInfo.w + "]");
00291         //return "";
00292         //}
00293         
00294         return "";
00295     }
00296 
00297 }
00298 


srs_knowledge
Author(s): Ze Ji
autogenerated on Mon Oct 6 2014 08:27:32