RosUtil.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@cf.ac.uk
00017  *
00018  * Date of creation: Oct 2011:
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 nor 
00032  *         the names of its contributors may be used to endorse or promote products 
00033  *         derived from this software without specific prior written permission.
00034  *
00035  * This program is free software: you can redistribute it and/or modify
00036  * it under the terms of the GNU Lesser General Public License LGPL as 
00037  * published by the Free Software Foundation, either version 3 of the 
00038  * License, or (at your option) any later version.
00039  * 
00040  * This program is distributed in the hope that it will be useful,
00041  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00042  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00043  * GNU Lesser General Public License LGPL for more details.
00044  * 
00045  * You should have received a copy of the GNU Lesser General Public 
00046  * License LGPL along with this program. 
00047  * If not, see <http://www.gnu.org/licenses/>.
00048  *
00049  ****************************************************************/
00050 
00051 package org.srs.srs_knowledge.knowledge_engine;
00052 
00053 import ros.*;
00054 import ros.communication.*;
00055 import ros.pkg.srs_knowledge.srv.GenerateSequence;
00056 import ros.pkg.srs_knowledge.srv.QuerySparQL;
00057 import ros.pkg.srs_knowledge.msg.*;
00058 import ros.pkg.srs_msgs.msg.SRSSpatialInfo;
00059 
00060 import ros.pkg.srs_knowledge.srv.PlanNextAction;
00061 import ros.pkg.srs_knowledge.srv.TaskRequest;
00062 import ros.pkg.srs_knowledge.srv.GetObjectsOnMap;
00063 import ros.pkg.srs_knowledge.srv.GetWorkspaceOnMap;
00064 import com.hp.hpl.jena.rdf.model.Statement;
00065 import org.srs.srs_knowledge.task.*;
00066 import ros.pkg.geometry_msgs.msg.Pose2D;
00067 import ros.pkg.geometry_msgs.msg.Pose;
00068 
00069 import java.io.*;
00070 
00071 public class RosUtil {
00072     public static Pose getRobotCurrentPose() {
00073         //Ros ros = Ros.getInstance();
00074         //ros.init("testKKKK");
00075         Pose pos = new Pose();
00076         //NodeHandle nn = ros.createNodeHandle();
00077         // /robot_pose_ekf/odom_combined
00078         try {
00079         Subscriber.QueueingCallback<ros.pkg.geometry_msgs.msg.PoseWithCovarianceStamped> callback =
00080             new Subscriber.QueueingCallback<ros.pkg.geometry_msgs.msg.PoseWithCovarianceStamped>();
00081         Subscriber<ros.pkg.geometry_msgs.msg.PoseWithCovarianceStamped> sub =
00082             KnowledgeEngine.nodeHandle.subscribe("/robot_pose_ekf/odom_combined", new ros.pkg.geometry_msgs.msg.PoseWithCovarianceStamped(), callback, 10);
00083         
00084         KnowledgeEngine.nodeHandle.spinOnce();
00085         while (!callback.isEmpty()) {
00086             pos = callback.pop().pose.pose;
00087             System.out.println(pos);
00088         }
00089         KnowledgeEngine.nodeHandle.shutdown();
00090         sub.shutdown();
00091         }
00092         catch(RosException e) {
00093             System.out.println(e.toString());
00094             return null;
00095         }
00096         catch(InterruptedException e)  {
00097             System.out.println(e.toString());
00098             return null;
00099         }
00100         return pos;
00101     }
00102     
00103     public static void testPub(java.lang.String c) throws RosException,Exception{
00104 
00105         Publisher<ros.pkg.std_msgs.msg.String> pub =
00106                KnowledgeEngine.nodeHandle.advertise("/pub", new ros.pkg.std_msgs.msg.String(), 100);
00107 
00108           ros.pkg.std_msgs.msg.String m = new ros.pkg.std_msgs.msg.String();
00109           m.data = c;
00110           for (int i = 0; i < 1000; i++) 
00111               {
00112                   pub.publish(m);
00113                   Thread.sleep(1000);
00114               }
00115           pub.shutdown();
00116          
00117     }
00118     public static java.lang.String testSub() throws RosException,Exception {
00119         Subscriber.QueueingCallback<ros.pkg.std_msgs.msg.String> callback =
00120             new Subscriber.QueueingCallback<ros.pkg.std_msgs.msg.String>();
00121         Subscriber<ros.pkg.std_msgs.msg.String> sub =
00122             KnowledgeEngine.nodeHandle.subscribe("/topic_name", new ros.pkg.std_msgs.msg.String(), callback, 10);
00123         
00124         KnowledgeEngine.nodeHandle.spinOnce();
00125         java.lang.String ret = "";
00126         while (!callback.isEmpty()) {
00127             ret = callback.pop().data;
00128             System.out.println(ret);
00129         }
00130         sub.shutdown();
00131         return ret;
00132     }
00133 
00134 }


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