$search
00001 /* 00002 * Copyright (C) 2010 by Moritz Tenorth 00003 * 00004 * This program is free software; you can redistribute it and/or modify 00005 * it under the terms of the GNU General Public License as published by 00006 * the Free Software Foundation; either version 3 of the License, or 00007 * (at your option) any later version. 00008 * 00009 * This program is distributed in the hope that it will be useful, 00010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 * GNU General Public License for more details. 00013 * 00014 * You should have received a copy of the GNU General Public License 00015 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 */ 00017 00018 package edu.tum.cs.ias.knowrob.tutorial; 00019 00020 import ros.*; 00021 import ros.pkg.geometry_msgs.msg.Point; 00022 import ros.pkg.geometry_msgs.msg.Quaternion; 00023 import ros.pkg.household_objects_database_msgs.msg.DatabaseModelPose; 00024 import ros.pkg.tabletop_object_detector.msg.*; 00025 import ros.pkg.tabletop_object_detector.srv.*; 00026 00027 00028 public class ROSClient { 00029 00030 static Boolean rosInitialized = false; 00031 static Ros ros; 00032 static NodeHandle n; 00033 00034 00040 public ROSClient(String node_name) { 00041 initRos(node_name); 00042 } 00043 00044 00050 protected static void initRos(String node_name) { 00051 00052 ros = Ros.getInstance(); 00053 00054 if(!Ros.getInstance().isInitialized()) { 00055 ros.init(node_name); 00056 } 00057 n = ros.createNodeHandle(); 00058 00059 } 00060 00061 00066 public TabletopDetectionResult callTabletopObjDetection() { 00067 00068 TabletopDetectionResult r=null; 00069 //try { 00070 00071 // TODO: call the tabletop_object_detector service 00072 // see also: http://www.ros.org/wiki/rosjava 00073 00074 00075 //} catch (RosException e) { 00076 // ros.logError("ROSClient: Call to service /object_detection failed"); 00077 //} 00078 00079 return r; 00080 } 00081 00087 public static double[] objectPose(DatabaseModelPose p) { 00088 00089 // TODO: implement 00090 return null; 00091 } 00092 00100 protected static double[] quaternionToMatrix(Point p, Quaternion q) { 00101 00102 double[] m = new double[16]; 00103 00104 double xx = q.x * q.x; 00105 double xy = q.x * q.y; 00106 double xz = q.x * q.z; 00107 double xw = q.x * q.w; 00108 00109 double yy = q.y * q.y; 00110 double yz = q.y * q.z; 00111 double yw = q.y * q.w; 00112 00113 double zz = q.z * q.z; 00114 double zw = q.z * q.w; 00115 00116 m[0] = 1 - 2 * ( yy + zz ); 00117 m[1] = 2 * ( xy - zw ); 00118 m[2] = 2 * ( xz + yw ); 00119 m[3] = p.x; 00120 00121 m[4] = 2 * ( xy + zw ); 00122 m[5] = 1 - 2 * ( xx + zz ); 00123 m[6] = 2 * ( yz - xw ); 00124 m[7] = p.y; 00125 00126 m[8] = 2 * ( xz - yw ); 00127 m[9] = 2 * ( yz + xw ); 00128 m[10] = 1 - 2 * ( xx + yy ); 00129 m[11]=p.z; 00130 00131 m[12]=0; 00132 m[13]=0; 00133 m[14]=0; 00134 m[15]=1; 00135 return m; 00136 } 00137 00138 00139 00140 /* 00141 * Test method: call the tabletop_object_detector and print results 00142 */ 00143 public static void main(String[] args) { 00144 00145 ROSClient r = new ROSClient("knowrob_tabletop_test_123"); 00146 00147 TabletopDetectionResult res = r.callTabletopObjDetection(); 00148 00149 for(int i=0;i<res.models.size();i++) { 00150 for(int j=0;j<res.models.get(i).model_list.size();j++) { 00151 System.out.println(res.models.get(i).model_list.get(j).model_id); 00152 System.out.println("Pos: "+res.models.get(i).model_list.get(j).pose.pose.position.x+","+res.models.get(i).model_list.get(j).pose.pose.position.y+","+res.models.get(i).model_list.get(j).pose.pose.position.z); 00153 } 00154 } 00155 } 00156 00157 } 00158 00159 00160