ROSClient.java

Go to the documentation of this file.

/*
 * Copyright (C) 2010 by Moritz Tenorth
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

package edu.tum.cs.ias.knowrob.tutorial;

import ros.*;
import ros.pkg.geometry_msgs.msg.Point;
import ros.pkg.geometry_msgs.msg.Quaternion;
import ros.pkg.household_objects_database_msgs.msg.DatabaseModelPose;
import ros.pkg.tabletop_object_detector.msg.*;
import ros.pkg.tabletop_object_detector.srv.*;


public class ROSClient {

        static Boolean rosInitialized = false;
        static Ros ros;
        static NodeHandle n;


        public ROSClient(String node_name) {
                initRos(node_name);
        }


        protected static void initRos(String node_name) {

                ros = Ros.getInstance();

                if(!Ros.getInstance().isInitialized()) {
                        ros.init(node_name);
                }
                n = ros.createNodeHandle();

        }


        public TabletopDetectionResult callTabletopObjDetection() {

                TabletopDetectionResult r=null;
    //try {

            // TODO: call the tabletop_object_detector service
            //       see also: http://www.ros.org/wiki/rosjava


    //} catch (RosException e) {
    //        ros.logError("ROSClient: Call to service /object_detection failed");
    //}

                return r;
        }

        public static double[] objectPose(DatabaseModelPose p) {

          // TODO: implement
                return null;
        }

        protected static double[] quaternionToMatrix(Point p, Quaternion q) {

            double[] m = new double[16];

            double xx = q.x * q.x;
            double xy = q.x * q.y;
            double xz = q.x * q.z;
            double xw = q.x * q.w;

            double yy = q.y * q.y;
            double yz = q.y * q.z;
            double yw = q.y * q.w;

            double zz = q.z * q.z;
            double zw = q.z * q.w;

            m[0]  = 1 - 2 * ( yy + zz );
            m[1]  =     2 * ( xy - zw );
            m[2]  =     2 * ( xz + yw );
            m[3]  = p.x;

            m[4]  =     2 * ( xy + zw );
            m[5]  = 1 - 2 * ( xx + zz );
            m[6]  =     2 * ( yz - xw );
            m[7]  = p.y;

            m[8]  =     2 * ( xz - yw );
            m[9]  =     2 * ( yz + xw );
            m[10] = 1 - 2 * ( xx + yy );
            m[11]=p.z;

            m[12]=0;
            m[13]=0;
            m[14]=0;
            m[15]=1;
            return m;
        }



        /*
         * Test method: call the tabletop_object_detector and print results
         */
        public static void main(String[] args) {

                ROSClient r = new ROSClient("knowrob_tabletop_test_123");

                TabletopDetectionResult res = r.callTabletopObjDetection();

                for(int i=0;i<res.models.size();i++) {
                        for(int j=0;j<res.models.get(i).model_list.size();j++) {
                                System.out.println(res.models.get(i).model_list.get(j).model_id);
                                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);
                        }
                }
        }

}