rosjava.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 // author: Jason Wolfe
00036 
00037 #include <list>
00038 #include <vector>
00039 #include <set>
00040 #include <string>
00041 #include <map>
00042 #include <sstream>
00043 #include <stdexcept>
00044 
00045 
00046 #include <cstdio>
00047 #include <boost/thread.hpp>
00048 #include <boost/thread/mutex.hpp>
00049 #include <boost/thread/condition.hpp>
00050 #include <boost/shared_ptr.hpp>
00051 #include <boost/thread/tss.hpp>
00052 
00053 
00054 #include <ros/init.h>
00055 #include <ros/time.h>
00056 #include <ros/master.h>
00057 #include <ros/this_node.h>
00058 #include <ros/node_handle.h>
00059 #include <ros/service.h>
00060 #include <rospack/rospack.h>
00061 
00062 #include <signal.h>
00063 
00064 #include "ros_roscpp_JNI.h"
00065 
00066 // I want assertions always on.
00067 #define MY_ROS_ASSERT(cond) \
00068   do { \
00069     if (!(cond)) { \
00070       ROS_FATAL("ASSERTION FAILED\n\tfile = %s\n\tline = %d\n\tcond = %s\n", __FILE__, __LINE__, #cond); \
00071       ROS_ISSUE_BREAK() \
00072     } \
00073   } while (0)
00074 
00075 
00076 
00077 #ifdef MSVC
00078 #define PRIdS "Id"
00079 #else
00080 #define PRIdS "zd"
00081 #endif
00082 
00083 using namespace ros;
00084 using namespace std;
00085 
00086 
00087 
00088 
00089 
00090 /******************************************************
00091  *   Misc Helpers
00092  ******************************************************/
00093 
00094 string getString(JNIEnv * env, jstring s)
00095 {
00096      if(s == NULL || env == NULL) return string("");
00097      const char* str = env->GetStringUTFChars(s, 0);
00098      string ret = str;
00099      env->ReleaseStringUTFChars(s, str);
00100 
00101      return ret;
00102 }
00103 
00104 jstring makeString(JNIEnv * env, const string &s) {
00105         return (jstring) env->NewStringUTF(s.c_str());
00106 }
00107 
00108 
00109 /******************************************************
00110  *   Thread-local JNIEnv objects
00111  ******************************************************/
00112 
00113 static JavaVM *vm = NULL;
00114 
00115 bool dieOnException(JNIEnv * env, const char * message = "") {
00116         if (env->ExceptionOccurred()) {
00117                 env->ExceptionDescribe();
00118                 ROS_FATAL(message);
00119                 MY_ROS_ASSERT(false);
00120                 return false;
00121         }
00122         return true;
00123 }
00124 
00125 class ThreadJNIEnv {
00126 public:
00127         bool _detach;
00128         JNIEnv *env;
00129         ThreadJNIEnv() {
00130                 cout << "Attaching " << boost::this_thread::get_id() << endl;
00131                 vm->AttachCurrentThread((void **) &env, NULL);
00132                 MY_ROS_ASSERT(env != NULL);
00133                 _detach = true;
00134 //              dieOnException(env, "Env already in exception state.");
00135         }
00136 
00137         ThreadJNIEnv(JNIEnv *e) {
00138                 env = e;
00139                 _detach = false;
00140                 MY_ROS_ASSERT(env != NULL);
00141         }
00142 
00143         ~ThreadJNIEnv() {
00144                 if (_detach) {
00145                         cout << "Detaching " << boost::this_thread::get_id() << endl;
00146                         vm->DetachCurrentThread();
00147                 }
00148         }
00149 };
00150 
00151 static boost::thread_specific_ptr<ThreadJNIEnv> tp;
00152 
00153 JNIEnv *getJNIEnv(){
00154         JNIEnv *ret;
00155 //      if (vm->GetEnv((void **)&ret, 0x00010001) == JNI_OK) {
00156 //              cout << "GOT ENV";
00157 //              return ret;
00158 //      }
00159 
00160         ThreadJNIEnv *tenv = tp.get();
00161         if (tenv == NULL) {
00162                 tenv = new ThreadJNIEnv();
00163                 tp.reset(tenv);
00164         }
00165 
00166         ret = tenv->env;
00167         MY_ROS_ASSERT(ret != NULL);
00168         dieOnException(ret, "Env already in exception state.");
00169         return ret;
00170 }
00171 
00172 
00173 
00174 
00175 /************************************************************
00176  *   Node Handle creation, running, destruction, and features
00177  ************************************************************/
00178 
00179 
00180 static jclass jObject;
00181 //static jclass jRuntimeException;
00182 static jclass jRosException;
00183 static jclass jTime;
00184 static jclass jString;
00185 static jclass jMessage;
00186 static jclass jSubscriberCallback;
00187 static jclass jServiceCallback;
00188 static jclass jByteBuffer;
00189 
00190 static jmethodID jTimeCtor;
00191 
00192 static jmethodID jMessageClone;
00193 static jmethodID jMessageGetDataType;
00194 static jmethodID jMessageGetMD5Sum;
00195 static jmethodID jMessageGetServerMD5Sum;
00196 static jmethodID jMessageGetMessageDefinition;
00197 static jmethodID jMessageSerializationLength;
00198 static jmethodID jMessageSerialize;
00199 static jmethodID jMessageDeserialize;
00200 static jmethodID jSubscriberCallbackCall;
00201 static jmethodID jServiceCallbackCall;
00202 static jmethodID jByteBufferOrder;
00203 
00204 static jobject jByteOrderLittleEndian;
00205 
00206 
00207 bool cacheClass(JNIEnv * env, jclass &ref, const char * name) {
00208         ref = env->FindClass(name);
00209         if (ref == NULL) return false;
00210         ref = (jclass) env->NewGlobalRef(ref);
00211         return true;
00212 }
00213 
00214 bool cacheMethod(JNIEnv * env, jmethodID &ref, jclass cls, const char * name, const char * args ) {
00215         ref = env->GetMethodID(cls, name, args);
00216         return (ref != NULL);
00217 }
00218 
00219 /*
00220  * Class:     ros_roscpp_CppNode
00221  * Method:    startNode
00222  * Signature: (Ljava/lang/Class;Ljava/lang/Class;Ljava/lang/Class;Ljava/lang/Class;Ljava/lang/Class;)V
00223  */
00224 
00225 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_init
00226   (JNIEnv * env, jclass __jni, jstring name, jboolean noSigintHandler, jboolean anonymousName, jboolean noRosout, jobjectArray args)
00227 {
00228         if (!env->GetJavaVM(&vm) < 0) return;
00229         tp.reset(new ThreadJNIEnv(env));
00230 
00231         if (!cacheClass(env, jObject, "java/lang/Object")) return; // Exception thrown
00232         if (!cacheClass(env, jString, "java/lang/String")) return; // Exception thrown
00233         //      if (!cacheClass(env, jRuntimeException, "java/lang/RuntimeException")) return; // Exception thrown
00234         if (!cacheClass(env, jRosException, "ros/RosException")) return; // Exception thrown
00235         if (!cacheClass(env, jTime, "ros/communication/Time")) return; // Exception thrown
00236         if (!cacheClass(env, jMessage, "ros/communication/Message")) return; // Exception thrown
00237         if (!cacheClass(env, jSubscriberCallback, "ros/Subscriber$Callback")) return; // Exception thrown
00238         if (!cacheClass(env, jServiceCallback, "ros/ServiceServer$Callback")) return; // Exception thrown
00239         if (!cacheClass(env, jByteBuffer, "java/nio/ByteBuffer")) return; // Exception thrown
00240 
00241         if (!cacheMethod(env, jTimeCtor, jTime, "<init>", "(II)V")) return; // Exception thrown
00242 
00243         if (!cacheMethod(env, jMessageClone, jMessage, "clone", "()Lros/communication/Message;")) return; // Exception thrown
00244         if (!cacheMethod(env, jMessageGetDataType, jMessage, "getDataType", "()Ljava/lang/String;")) return; // Exception thrown
00245         if (!cacheMethod(env, jMessageGetMD5Sum, jMessage, "getMD5Sum", "()Ljava/lang/String;")) return; // Exception thrown
00246         if (!cacheMethod(env, jMessageGetServerMD5Sum, jMessage, "getServerMD5Sum", "()Ljava/lang/String;")) return; // Exception thrown
00247         if (!cacheMethod(env, jMessageGetMessageDefinition, jMessage, "getMessageDefinition", "()Ljava/lang/String;")) return; // Exception thrown
00248         if (!cacheMethod(env, jMessageSerializationLength, jMessage, "serializationLength", "()I")) return; // Exception thrown
00249         if (!cacheMethod(env, jMessageSerialize, jMessage, "serialize", "(Ljava/nio/ByteBuffer;I)V")) return; // Exception thrown
00250         if (!cacheMethod(env, jMessageDeserialize, jMessage, "deserialize", "(Ljava/nio/ByteBuffer;)V")) return; // Exception thrown
00251         if (!cacheMethod(env, jSubscriberCallbackCall, jSubscriberCallback, "call", "(Lros/communication/Message;)V")) return; // Exception thrown
00252         if (!cacheMethod(env, jServiceCallbackCall, jServiceCallback, "call", "(Lros/communication/Message;)Lros/communication/Message;")) return; // Exception thrown
00253         if (!cacheMethod(env, jByteBufferOrder, jByteBuffer, "order", "(Ljava/nio/ByteOrder;)Ljava/nio/ByteBuffer;")) return; // Exception thrown
00254 
00255         jclass jByteOrder = env->FindClass("java/nio/ByteOrder");
00256         if (jByteOrder == NULL) return;
00257         jfieldID fid      = env->GetStaticFieldID(jByteOrder, "LITTLE_ENDIAN", "Ljava/nio/ByteOrder;");
00258         if (fid == NULL) return;
00259         jByteOrderLittleEndian = env->NewGlobalRef(env->GetStaticObjectField(jByteOrder, fid));
00260         if (jByteOrderLittleEndian == NULL) return;
00261 
00262 
00263         int len = env->GetArrayLength(args);
00264 
00265         vector<string> vargs;
00266         for(int i = 0; i < len; i++) vargs.push_back(getString(env, (jstring) env->GetObjectArrayElement(args,i)));
00267 
00268     vector<char*> argv(len);
00269     for(int i = 0; i < len; ++i)
00270         argv[i] = &vargs[i][0];
00271 
00272     uint32_t options = (noSigintHandler ? ros::init_options::NoSigintHandler : 0) |
00273                            (anonymousName   ? ros::init_options::AnonymousName : 0) |
00274                            (noRosout        ? ros::init_options::NoRosout      : 0);
00275     ros::init(len, len > 0 ? &argv[0] : NULL, getString(env, name), options);
00276 }
00277 
00278 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_createNodeHandle
00279   (JNIEnv * env, jclass __jni, jstring jns, jobjectArray jmappings)
00280 {
00281         map<string, string> remappings;
00282         int len = env->GetArrayLength(jmappings);
00283         MY_ROS_ASSERT(len % 2 == 0);
00284         for(int i = 0; i < len; i+=2)
00285                 remappings[getString(env, (jstring) env->GetObjectArrayElement(jmappings, i))] =
00286                                    getString(env, (jstring) env->GetObjectArrayElement(jmappings, i+1));
00287 
00288         return (long) (new NodeHandle(getString(env, jns), remappings));
00289 }
00290 
00291 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_shutdown
00292   (JNIEnv * env, jclass __jni, jlong cppHandle)
00293 {
00294 //    ROS_INFO("exiting rosjava");
00295 
00296 /*    env->DeleteGlobalRef(jObject);
00297         env->DeleteGlobalRef(jString);
00298         env->DeleteGlobalRef(jRosException);
00299         env->DeleteGlobalRef(jTime);
00300         env->DeleteGlobalRef(jMessage);
00301         env->DeleteGlobalRef(jSubscriberCallback);
00302         env->DeleteGlobalRef(jServiceCallback);*/
00303     // Since init is one-time right now, just never release these ...
00304 
00305     NodeHandle *handle = (NodeHandle *) cppHandle;
00306         handle->shutdown();
00307         delete handle;
00308 }
00309 
00310 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_nhOk
00311   (JNIEnv * env, jclass __jni, jlong cppHandle)
00312 {
00313   NodeHandle *handle = (NodeHandle *) cppHandle;
00314         return handle->ok();
00315 }
00316 
00317 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_rosOk
00318   (JNIEnv * env, jclass __jni)
00319 {
00320         return ros::ok();
00321 }
00322 
00323 
00324 
00325 
00326 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_spin
00327   (JNIEnv * env, jclass __jni)
00328 {
00329         ros::spin();
00330 }
00331 
00332 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_spinOnce
00333   (JNIEnv * env, jclass __jni)
00334 {
00335         ros::spinOnce();
00336 }
00337 
00338 
00339 
00340 
00341 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_checkMaster
00342   (JNIEnv * env, jclass __jni, jlong cppHandle)
00343 {
00344        return ros::master::check();
00345 }
00346 
00347 JNIEXPORT jstring JNICALL Java_ros_roscpp_JNI_getMasterHost
00348   (JNIEnv * env, jclass __jni, jlong cppHandle)
00349 {
00350         //NodeHandle *handle = (NodeHandle *) cppHandle;
00351         return makeString(env, ros::master::getHost());
00352 }
00353 
00354 JNIEXPORT jint JNICALL Java_ros_roscpp_JNI_getMasterPort
00355   (JNIEnv * env, jclass __jni, jlong cppHandle)
00356 {
00357         //NodeHandle *handle = (NodeHandle *) cppHandle;
00358         return ros::master::getPort();
00359 }
00360 
00361 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_setMasterRetryTimeout
00362   (JNIEnv * env, jclass __jni, jlong cppHandle, jint ms)
00363 {
00364 //      NodeHandle *handle = (NodeHandle *) cppHandle;
00365         ros::master::setRetryTimeout(WallDuration((ms/1000),(ms % 1000) * 1000000));
00366 }
00367 
00368 
00369 
00370 
00371 JNIEXPORT jstring JNICALL Java_ros_roscpp_JNI_getName
00372   (JNIEnv * env, jclass __jni, jlong cppHandle)
00373 {
00374 //      NodeHandle *handle = (NodeHandle *) cppHandle;
00375 //      return makeString(env, handle->getName());
00376         return makeString(env, ros::this_node::getName());
00377 }
00378 
00379 
00380 JNIEXPORT jstring JNICALL Java_ros_roscpp_JNI_mapName
00381   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jname)
00382 {
00383         NodeHandle *handle = (NodeHandle *) cppHandle;
00384         return makeString(env, handle->resolveName(getString(env, jname)));
00385 }
00386 
00387 
00388 JNIEXPORT jstring JNICALL Java_ros_roscpp_JNI_getPackageLocation
00389 (JNIEnv * env, jclass __jni, jstring jname)
00390 {
00391         string pkgName = getString(env,jname);
00392 
00393         char a1[21], a2[21], a3[1025];
00394         snprintf(a1, 20, "rospack");
00395         snprintf(a2, 20, "find");
00396         snprintf(a3, 1024, "%s", pkgName.c_str());
00397         char* argv[3] = {a1, a2, a3};
00398         std::string retval = "";
00399         try  {
00400                 rospack::ROSPack rp;
00401                 rp.run(3, argv);
00402                 retval = rp.getOutput();
00403                 retval.resize(retval.size()-1);
00404         }
00405         catch(std::runtime_error &e) {
00406                 fprintf(stderr, "[rospack] %s\n", e.what());
00407         }
00408 
00409         return makeString(env, retval);
00410 }
00411 
00412 /************************************************************
00413  *   Logging
00414  ************************************************************/
00415 
00416 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_logDebug
00417   (JNIEnv * env, jclass __jni, jstring jmessage)
00418 {
00419         ROS_DEBUG(getString(env,jmessage).c_str());
00420 }
00421 
00422 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_logInfo
00423   (JNIEnv * env, jclass __jni, jstring jmessage)
00424 {
00425         ROS_INFO(getString(env,jmessage).c_str());
00426 }
00427 
00428 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_logWarn
00429   (JNIEnv * env, jclass __jni, jstring jmessage)
00430 {
00431         ROS_WARN(getString(env,jmessage).c_str());
00432 }
00433 
00434 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_logError
00435   (JNIEnv * env, jclass __jni, jstring jmessage)
00436 {
00437         ROS_ERROR(getString(env,jmessage).c_str());
00438 }
00439 
00440 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_logFatal
00441   (JNIEnv * env, jclass __jni, jstring jmessage)
00442 {
00443         ROS_FATAL(getString(env,jmessage).c_str());
00444 }
00445 
00446 
00447 
00448 
00449 /************************************************************
00450  *   Getting miscellaneous info
00451  ************************************************************/
00452 
00453 JNIEXPORT jobject JNICALL Java_ros_roscpp_JNI_now
00454   (JNIEnv * env, jclass __jni) {
00455     ros::Time t = ros::Time::now();
00456         jobject ret = env->NewObject(jTime, jTimeCtor, t.sec, t.nsec);
00457     return ret;
00458 }
00459 
00460 JNIEXPORT jobjectArray JNICALL Java_ros_roscpp_JNI_getSubscribedTopics
00461   (JNIEnv * env, jclass __jni, jlong cppHandle)
00462 {
00463         //NodeHandle *handle = (NodeHandle *) cppHandle;
00464 
00465     vector<string> topics;
00466     ros::this_node::getSubscribedTopics(topics);
00467 
00468     int sz = topics.size();
00469     jobjectArray oa = env->NewObjectArray(sz, jString, env->NewStringUTF(""));
00470     if (oa == NULL) return NULL;
00471 
00472     for(int i = 0; i < sz; i++) {
00473         env->SetObjectArrayElement(oa, i, makeString(env, topics[i]));
00474     }
00475 
00476         return oa;
00477 }
00478 
00479 JNIEXPORT jobjectArray JNICALL Java_ros_roscpp_JNI_getAdvertisedTopics
00480   (JNIEnv * env, jclass __jni, jlong cppHandle)
00481 {
00482         //NodeHandle *handle = (NodeHandle *) cppHandle;
00483 
00484     vector<string> topics;
00485     ros::this_node::getAdvertisedTopics(topics);
00486 
00487     int sz = topics.size();
00488     jobjectArray oa = env->NewObjectArray(sz, jString, env->NewStringUTF(""));
00489     if (oa == NULL) return NULL;
00490 
00491     for(int i = 0; i < sz; i++) {
00492         env->SetObjectArrayElement(oa, i, makeString(env, topics[i]));
00493     }
00494 
00495         return oa;
00496 }
00497 
00498 JNIEXPORT jobjectArray JNICALL Java_ros_roscpp_JNI_getPublishedTopics
00499   (JNIEnv * env, jclass __jni, jlong cppHandle)
00500 {
00501         //NodeHandle *handle = (NodeHandle *) cppHandle;
00502 
00503     vector<ros::master::TopicInfo> vtopics;
00504     ros::master::getTopics(vtopics);
00505 
00506     int sz = vtopics.size();
00507     jobjectArray oa = env->NewObjectArray(sz * 3, jString, env->NewStringUTF(""));
00508     if (oa == NULL) return NULL;
00509 
00510     for(int i = 0; i < sz; i++) {
00511         env->SetObjectArrayElement(oa, i * 3, env->NewStringUTF(vtopics[i].name.c_str()));
00512         env->SetObjectArrayElement(oa, i * 3 + 1, env->NewStringUTF(vtopics[i].datatype.c_str()));
00513         // The TopicInfo structure no longer has an md5sum.
00514         //env->SetObjectArrayElement(oa, i * 3 + 2, env->NewStringUTF(vtopics[i].md5sum.c_str()));
00515         env->SetObjectArrayElement(oa, i * 3 + 2, env->NewStringUTF(""));
00516     }
00517 
00518         return oa;
00519 }
00520 
00521 
00522 /************************************************************
00523  *   Parameters
00524  ************************************************************/
00525 
00526 
00527 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_hasParam
00528   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam)
00529 {
00530         NodeHandle *handle = (NodeHandle *) cppHandle;
00531         return handle->hasParam(getString(env, jparam));
00532 }
00533 
00534 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_getBooleanParam
00535   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jboolean cache)
00536 {
00537         NodeHandle *handle = (NodeHandle *) cppHandle;
00538         bool ret;
00539         if (!handle->getParam(getString(env, jparam), ret)) {
00540         env->ThrowNew(jRosException, "Param could not be fetched!");
00541         return false;
00542         }
00543         return ret;
00544 }
00545 
00546 JNIEXPORT jint JNICALL Java_ros_roscpp_JNI_getIntParam
00547   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jboolean cache)
00548 {
00549         NodeHandle *handle = (NodeHandle *) cppHandle;
00550         int ret;
00551         if (!handle->getParam(getString(env, jparam), ret)) {
00552         env->ThrowNew(jRosException, "Param could not be fetched!");
00553         return 0;
00554         }
00555         return ret;
00556 }
00557 
00558 JNIEXPORT jdouble JNICALL Java_ros_roscpp_JNI_getDoubleParam
00559   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jboolean cache)
00560 {
00561         NodeHandle *handle = (NodeHandle *) cppHandle;
00562         double ret;
00563         if (!handle->getParam(getString(env, jparam), ret)) {
00564         env->ThrowNew(jRosException, "Param could not be fetched!");
00565         return 0;
00566         }
00567         return ret;
00568 }
00569 
00570 JNIEXPORT jstring JNICALL Java_ros_roscpp_JNI_getStringParam
00571   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jboolean cache)
00572 {
00573         NodeHandle *handle = (NodeHandle *) cppHandle;
00574         string ret;
00575         if (!handle->getParam(getString(env, jparam), ret)) {
00576         env->ThrowNew(jRosException, "Param could not be fetched!");
00577         return 0;
00578         }
00579         return makeString(env, ret);
00580 }
00581 
00582 
00583 
00584 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_setParam__JLjava_lang_String_2Z
00585   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jboolean val)
00586 {
00587         NodeHandle *handle = (NodeHandle *) cppHandle;
00588         return handle->setParam(getString(env, jparam), val);
00589 }
00590 
00591 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_setParam__JLjava_lang_String_2I
00592   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jint val)
00593 {
00594         NodeHandle *handle = (NodeHandle *) cppHandle;
00595         return handle->setParam(getString(env, jparam), (int)val);
00596 }
00597 
00598 
00599 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_setParam__JLjava_lang_String_2D
00600   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jdouble val)
00601 {
00602         NodeHandle *handle = (NodeHandle *) cppHandle;
00603         return handle->setParam(getString(env, jparam), val);
00604 }
00605 
00606 
00607 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_setParam__JLjava_lang_String_2Ljava_lang_String_2
00608   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jparam, jstring val)
00609 {
00610         NodeHandle *handle = (NodeHandle *) cppHandle;
00611         return handle->setParam(getString(env, jparam), getString(env, val));
00612 }
00613 
00614 
00615 
00616 
00617 
00618 /************************************************************
00619  *   Message wrapper
00620  ************************************************************/
00621 
00622 
00623 class JavaMessage
00624 {
00625 public:
00626     jobject _message;
00627 
00628     JavaMessage(jobject message) : _message(getJNIEnv()->NewGlobalRef(message)) {}
00629 
00630     JavaMessage(const JavaMessage& r)
00631     {
00632 //      cout << "CLONE" << endl;
00633         JNIEnv * env = getJNIEnv();
00634         _message = env->CallObjectMethod(r._message,jMessageClone);
00635         MY_ROS_ASSERT(_message && dieOnException(env));
00636         _message = env->NewGlobalRef(_message);
00637     }
00638 
00639     virtual ~JavaMessage() {
00640         getJNIEnv()->DeleteGlobalRef(_message);
00641     }
00642 
00643     virtual void replaceContents (jobject newMessage) {
00644  //     cout << "Replace!" << endl;
00645         JNIEnv * env = getJNIEnv();
00646         env->DeleteGlobalRef(_message);
00647         _message = env->NewGlobalRef(newMessage);
00648         MY_ROS_ASSERT(_message && dieOnException(env));
00649     }
00650 
00651     virtual const string __getDataType() const {
00652 //              cout << "getDataType:";
00653         JNIEnv * env = getJNIEnv();
00654         jstring s = (jstring) env->CallObjectMethod(_message, jMessageGetDataType);
00655         MY_ROS_ASSERT(s && dieOnException(env));
00656 //      cout << getString(env, s).c_str() << endl;
00657         return getString(env, s);
00658     }
00659 
00660     virtual const string __getMD5Sum()   const {
00661 //              cout << "getMD5:";
00662         JNIEnv * env = getJNIEnv();
00663         jstring s = (jstring) env->CallObjectMethod(_message, jMessageGetMD5Sum);
00664         MY_ROS_ASSERT(s && dieOnException(env));
00665 //      cout << getString(env, s).c_str() << endl;
00666         return getString(env, s);
00667     }
00668 
00669     virtual const string __getServerMD5Sum()   const {
00670 //              cout << "getMD5:";
00671         JNIEnv * env = getJNIEnv();
00672         jstring s = (jstring) env->CallObjectMethod(_message, jMessageGetServerMD5Sum);
00673         MY_ROS_ASSERT(s && dieOnException(env));
00674 //      cout << getString(env, s).c_str() << endl;
00675         return getString(env, s);
00676     }
00677 
00678     virtual const string __getMessageDefinition()   const {
00679 //              cout << "getMD: ";
00680         JNIEnv * env = getJNIEnv();
00681         jstring s = (jstring) env->CallObjectMethod(_message, jMessageGetMessageDefinition);
00682         MY_ROS_ASSERT(s && dieOnException(env));
00683 //      cout << getString(env, s).c_str() << endl;
00684         return getString(env, s);
00685     }
00686 
00687     uint32_t serializationLength() const {
00688 //              cout << "serialLength:" << endl;
00689         JNIEnv * env = getJNIEnv();
00690         int len =  getJNIEnv()->CallIntMethod(_message, jMessageSerializationLength);
00691         dieOnException(env);
00692  //     cout << len << endl;
00693         return len;
00694     }
00695 
00696     virtual uint8_t *serialize(uint8_t *writePtr, uint32_t seqid) const {
00697 //              cout << "serialize" << endl;
00698         JNIEnv * env = getJNIEnv();
00699         uint32_t len = serializationLength();
00700         jobject bb = env->NewDirectByteBuffer(writePtr, len);
00701         MY_ROS_ASSERT(bb && dieOnException(env));
00702         bb = env->CallObjectMethod(bb, jByteBufferOrder, jByteOrderLittleEndian);
00703         MY_ROS_ASSERT(bb && dieOnException(env));
00704         env->CallObjectMethod(_message, jMessageSerialize, bb, seqid);
00705         dieOnException(env);
00706         return writePtr + len;
00707     }
00708 
00709     virtual uint8_t *deserialize(uint8_t *readPtr, uint32_t sz) {
00710 //              cout << "deserialize" << endl;
00711         JNIEnv * env = getJNIEnv();
00712         if (sz == 0) {
00713 //              ROS_WARN("empty message!");
00714                 return readPtr;
00715         }
00716         jobject bb = env->NewDirectByteBuffer(readPtr, sz);
00717         MY_ROS_ASSERT(bb && dieOnException(env));
00718         bb = env->CallObjectMethod(bb, jByteBufferOrder, jByteOrderLittleEndian);
00719         MY_ROS_ASSERT(bb && dieOnException(env));
00720         env->CallObjectMethod(_message, jMessageDeserialize, bb);
00721         dieOnException(env);
00722         return readPtr + sz;
00723     }
00724 };
00725 
00726 namespace ros
00727 {
00728 
00729 namespace serialization
00730 {
00731 
00732 template<>
00733 struct Serializer<JavaMessage>
00734 {
00738   template<typename Stream>
00739   inline static void write(Stream& stream, boost::call_traits<JavaMessage>::param_type t)
00740   {
00741     t.serialize(stream.getData(), 0);
00742   }
00743 
00747   template<typename Stream>
00748   inline static void read(Stream& stream, boost::call_traits<JavaMessage>::reference t)
00749   {
00750     t.deserialize(stream.getData(), stream.getLength());
00751   }
00752 
00756   inline static uint32_t serializedLength(boost::call_traits<JavaMessage>::param_type t)
00757   {
00758     return t.serializationLength();
00759   }
00760 };
00761 
00762 }
00763 }
00764 
00765 /************************************************************
00766  *   Subscriptions
00767  ************************************************************/
00768 
00769 #if ROS_NEW_SERIALIZATION_API
00770 class SubscriptionMessage
00771 {
00772 public:
00773     jobject _scb;
00774     JavaMessage _msg;
00775     string md5, datatype;
00776 
00777     SubscriptionMessage(jobject scb, jobject tmpl) : _scb(getJNIEnv()->NewGlobalRef(scb)), _msg(tmpl) {
00778         md5 = _msg.__getMD5Sum();
00779         datatype = _msg.__getDataType();
00780     }
00781 
00782     ~SubscriptionMessage() { getJNIEnv()->DeleteGlobalRef(_scb); }
00783 
00784     boost::shared_ptr<JavaMessage> create() { return boost::shared_ptr<JavaMessage>(new JavaMessage(_msg)); }
00785     void callback(const boost::shared_ptr<JavaMessage const>& msg)
00786     {
00787       getJNIEnv()->CallVoidMethod(_scb, jSubscriberCallbackCall, msg->_message);
00788     }
00789 
00790     std::string getMD5Sum() { return md5; }
00791     std::string getDataType() { return datatype; }
00792 };
00793 #else
00794 class JavaSubscriptionMessageHelper : public ros::SubscriptionMessageHelper {
00795 public:
00796         jobject _scb;
00797         JavaMessage _msg;
00798         string md5, datatype;
00799 
00800         JavaSubscriptionMessageHelper(jobject scb, jobject tmpl) : _scb(getJNIEnv()->NewGlobalRef(scb)), _msg(tmpl) {
00801                 md5 = _msg.__getMD5Sum();
00802                 datatype = _msg.__getDataType();
00803         }
00804         ~JavaSubscriptionMessageHelper() { getJNIEnv()->DeleteGlobalRef(_scb); }
00805 
00806         virtual MessagePtr create() { return boost::shared_ptr<Message>(new JavaMessage(_msg)); }
00807 
00808         virtual std::string getMD5Sum() { return md5; }
00809         virtual std::string getDataType() { return datatype; }
00810 
00811         virtual void call(const MessagePtr &msg) {
00812         getJNIEnv()->CallVoidMethod(_scb, jSubscriberCallbackCall, ((JavaMessage *)msg.get())->_message);
00813         }
00814 };
00815 #endif
00816 
00817 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_createSubCallback
00818   (JNIEnv * env, jclass __jni, jobject jcallback, jobject messageTemplate)
00819 {
00820 #if ROS_NEW_SERIALIZATION_API
00821     return (jlong) new boost::shared_ptr<SubscriptionMessage>(new SubscriptionMessage(jcallback, messageTemplate));
00822 #else
00823         return (jlong) new boost::shared_ptr<JavaSubscriptionMessageHelper>(new JavaSubscriptionMessageHelper(jcallback, messageTemplate));
00824 #endif
00825 }
00826 
00827 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_deleteSubCallback
00828   (JNIEnv * env, jclass __jni, jlong cppCallback)
00829 {
00830 #if ROS_NEW_SERIALIZATION_API
00831     boost::shared_ptr<SubscriptionMessage> *callback = (boost::shared_ptr<SubscriptionMessage> *) cppCallback;
00832     delete callback;
00833 #else
00834     boost::shared_ptr<JavaSubscriptionMessageHelper> *callback = (boost::shared_ptr<JavaSubscriptionMessageHelper> *) cppCallback;
00835     delete callback;
00836 #endif
00837 }
00838 
00839 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_subscribe
00840   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jtopic, jlong cppCallback, jint queueSize)
00841 {
00842         NodeHandle *handle = (NodeHandle *) cppHandle;
00843 #if ROS_NEW_SERIALIZATION_API
00844         boost::shared_ptr<SubscriptionMessage> *callback = (boost::shared_ptr<SubscriptionMessage> *) cppCallback;
00845         SubscribeOptions so(getString(env, jtopic), queueSize, (*callback)->getMD5Sum(), (*callback)->getDataType());
00846         so.helper.reset(new SubscriptionCallbackHelperT<const boost::shared_ptr<JavaMessage const>&>(boost::bind(&SubscriptionMessage::callback, *callback, _1),
00847                                                                 boost::bind(&SubscriptionMessage::create, *callback)));
00848 #else
00849         boost::shared_ptr<SubscriptionMessageHelper> *callback = (boost::shared_ptr<SubscriptionMessageHelper> *) cppCallback;
00850         SubscribeOptions so(getString(env, jtopic), queueSize, *callback);
00851 #endif
00852 
00853 
00854         Subscriber subscriber = handle->subscribe(so);
00855         if (subscriber) return (jlong) new Subscriber(subscriber);
00856         return 0;
00857 }
00858 
00859 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_isSubscriberValid
00860   (JNIEnv * env, jclass __jni, jlong cppSubscriber)
00861 {
00862         Subscriber *subscriber = (Subscriber *) cppSubscriber;
00863         return (*subscriber) && ros::ok() ? true : false;
00864 }
00865 
00866 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_shutdownSubscriber
00867   (JNIEnv * env, jclass __jni, jlong cppSubscriber)
00868 {
00869         Subscriber *subscriber = (Subscriber *) cppSubscriber;
00870         delete subscriber;
00871 }
00872 
00873 
00874 
00875 /************************************************************
00876  *   Publications
00877  ************************************************************/
00878 
00879 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_advertise
00880   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring jtopic, jobject jmessage, jint queueSize, jboolean latch)
00881 {
00882         NodeHandle *handle = (NodeHandle *) cppHandle;
00883         JavaMessage msg(jmessage);
00884         AdvertiseOptions ao(getString(env, jtopic), queueSize, msg.__getMD5Sum(), msg.__getDataType(), msg.__getMessageDefinition());
00885         ao.latch = latch;
00886         Publisher publisher = handle->advertise(ao);
00887         if (publisher) return (jlong) new Publisher(publisher);
00888         return 0;
00889 }
00890 
00891 JNIEXPORT jint JNICALL Java_ros_roscpp_JNI_getNumSubscribers
00892   (JNIEnv * env, jclass __jni, jlong cppPublisher) {
00893         Publisher *publisher = (Publisher *) cppPublisher;
00894         return publisher->getNumSubscribers();
00895 }
00896 
00897 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_publish
00898   (JNIEnv * env, jclass __jni, jlong cppPublisher, jobject jmessage) {
00899         Publisher *publisher = (Publisher *) cppPublisher;
00900         JavaMessage message(jmessage);
00901         publisher->publish(message);
00902 }
00903 
00904 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_isPublisherValid
00905   (JNIEnv * env, jclass __jni, jlong cppPublisher)
00906 {
00907         Publisher *publisher = (Publisher *) cppPublisher;
00908         return (*publisher)  && ros::ok() ? true : false;
00909 }
00910 
00911 
00912 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_shutdownPublisher
00913   (JNIEnv * env, jclass __jni, jlong cppPublisher)
00914 {
00915         Publisher *publisher = (Publisher *) cppPublisher;
00916         delete publisher;
00917 }
00918 
00919 
00920 
00921 /************************************************************
00922  *   Service Clients
00923  ************************************************************/
00924 
00925 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_serviceClient
00926   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring name, jstring md5, jboolean persist, jobjectArray jheaderValues)
00927 {
00928         NodeHandle *handle = (NodeHandle *) cppHandle;
00929         map<string, string> headers;
00930         int len = env->GetArrayLength(jheaderValues);
00931         for(int i = 0; i < len; i+=2) headers[getString(env, (jstring) env->GetObjectArrayElement(jheaderValues,i))] = getString(env, (jstring) env->GetObjectArrayElement(jheaderValues,i+1));
00932 
00933         ServiceClientOptions sco(getString(env, name), getString(env, md5), persist, headers);
00934         ServiceClient client = handle->serviceClient(sco);
00935         if (client) return (jlong) new ServiceClient(client);
00936         return 0;
00937 }
00938 
00939 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_callService
00940   (JNIEnv * env, jclass __jni, jlong cppServiceClient, jobject jrequest, jobject jresponse, jstring md5)
00941 {
00942         ServiceClient *client = (ServiceClient *) cppServiceClient;
00943         JavaMessage request(jrequest), response(jresponse);
00944 //      cout << "about to call!" << endl;
00945         return client->call(request, response, getString(env, md5));
00946 }
00947 
00948 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_shutdownServiceClient
00949   (JNIEnv * env, jclass __jni, jlong cppServiceClient)
00950 {
00951         ServiceClient *client = (ServiceClient *) cppServiceClient;
00952         delete client;
00953 }
00954 
00955 
00956 /************************************************************
00957  *   Service Servers
00958  ************************************************************/
00959 
00960 #if ROS_NEW_SERIALIZATION_API
00961 class ServiceMessage
00962 {
00963 public:
00964     jobject _scb;
00965     JavaMessage _req, _res;
00966     string md5, datatype, requestDataType, responseDataType;
00967 
00968     ServiceMessage(jobject scb, string smd5, string sdatatype, jobject req, jobject res)
00969         : _scb(getJNIEnv()->NewGlobalRef(scb)), _req(req), _res(res), md5(smd5), datatype(sdatatype) {
00970         requestDataType = _req.__getDataType();
00971         responseDataType = _res.__getDataType();
00972     }
00973     ~ServiceMessage() { getJNIEnv()->DeleteGlobalRef(_scb); }
00974 
00975     boost::shared_ptr<JavaMessage> createRequest() { return boost::shared_ptr<JavaMessage>(new JavaMessage(_req)); }
00976     boost::shared_ptr<JavaMessage> createResponse() { return boost::shared_ptr<JavaMessage>(new JavaMessage(_res)); }
00977 
00978     std::string getMD5Sum() { return md5; }
00979     std::string getDataType() { return datatype; }
00980     std::string getRequestDataType() { return requestDataType; }
00981     std::string getResponseDataType() { return responseDataType; }
00982 
00983     bool callback(JavaMessage& req, JavaMessage& res) {
00984         JNIEnv * env = getJNIEnv();
00985         jobject jresponse = env->CallObjectMethod(_scb, jServiceCallbackCall, req._message);
00986         MY_ROS_ASSERT(jresponse != 0 && dieOnException(env));
00987         res.replaceContents(jresponse);
00988         return true;
00989     }
00990 };
00991 #else
00992 class JavaServiceMessageHelper : public ros::ServiceMessageHelper {
00993 public:
00994         jobject _scb;
00995         JavaMessage _req, _res;
00996         string md5, datatype, requestDataType, responseDataType;
00997 
00998         JavaServiceMessageHelper(jobject scb, string smd5, string sdatatype, jobject req, jobject res)
00999                 : _scb(getJNIEnv()->NewGlobalRef(scb)), _req(req), _res(res), md5(smd5), datatype(sdatatype) {
01000                 requestDataType = _req.__getDataType();
01001                 responseDataType = _res.__getDataType();
01002         }
01003         ~JavaServiceMessageHelper() { getJNIEnv()->DeleteGlobalRef(_scb); }
01004 
01005         virtual MessagePtr createRequest() { return boost::shared_ptr<Message>(new JavaMessage(_req)); }
01006         virtual MessagePtr createResponse() { return boost::shared_ptr<Message>(new JavaMessage(_res)); }
01007 
01008         virtual std::string getMD5Sum() { return md5; }
01009         virtual std::string getDataType() { return datatype; }
01010         virtual std::string getRequestDataType() { return requestDataType; }
01011         virtual std::string getResponseDataType() { return responseDataType; }
01012 
01013         virtual bool call(const MessagePtr &req, const MessagePtr &res) {
01014         JNIEnv * env = getJNIEnv();
01015         jobject jresponse = env->CallObjectMethod(_scb, jServiceCallbackCall, ((JavaMessage *)req.get())->_message);
01016         MY_ROS_ASSERT(jresponse != 0 && dieOnException(env));
01017         ((JavaMessage *)res.get())->replaceContents(jresponse);
01018         return true;
01019         }
01020 };
01021 #endif
01022 
01023 
01024 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_createSrvCallback
01025   (JNIEnv * env, jclass __jni, jobject jcallback, jstring serviceMD5, jstring serviceDataType, jobject jreq, jobject jres)
01026 {
01027 #if ROS_NEW_SERIALIZATION_API
01028     ServiceMessage *helper = new ServiceMessage(jcallback, getString(env, serviceMD5), getString(env, serviceDataType), jreq, jres);
01029     return (jlong) new boost::shared_ptr<ServiceMessage>(helper);
01030 #else
01031         JavaServiceMessageHelper *helper = new JavaServiceMessageHelper(jcallback, getString(env, serviceMD5), getString(env, serviceDataType), jreq, jres);
01032         return (jlong) new boost::shared_ptr<JavaServiceMessageHelper>(helper);
01033 #endif
01034 }
01035 
01036 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_deleteSrvCallback
01037   (JNIEnv * env, jclass __jni, jlong cppCallback)
01038 {
01039 #if ROS_NEW_SERIALIZATION_API
01040     boost::shared_ptr<ServiceMessage> *callback = (boost::shared_ptr<ServiceMessage> *) cppCallback;
01041     delete callback;
01042 #else
01043         boost::shared_ptr<JavaServiceMessageHelper> *callback = (boost::shared_ptr<JavaServiceMessageHelper> *) cppCallback;
01044         delete callback;
01045 #endif
01046 }
01047 
01048 JNIEXPORT jlong JNICALL Java_ros_roscpp_JNI_advertiseService
01049   (JNIEnv * env, jclass __jni, jlong cppHandle, jstring name, jlong cppCallback)
01050 {
01051         NodeHandle *handle = (NodeHandle *) cppHandle;
01052 #if ROS_NEW_SERIALIZATION_API
01053         boost::shared_ptr<ServiceMessage> *callback = (boost::shared_ptr<ServiceMessage> *) cppCallback;
01054         AdvertiseServiceOptions aso;
01055         aso.service = getString(env, name);
01056         aso.md5sum = (*callback)->getMD5Sum();
01057         aso.datatype = (*callback)->getDataType();
01058         aso.req_datatype = (*callback)->getRequestDataType();
01059         aso.res_datatype = (*callback)->getResponseDataType();
01060         aso.helper.reset(new ServiceCallbackHelperT<ServiceSpec<JavaMessage, JavaMessage> >(boost::bind(&ServiceMessage::callback, *callback, _1, _2),
01061                                                                          boost::bind(&ServiceMessage::createRequest, *callback),
01062                                                                          boost::bind(&ServiceMessage::createResponse, *callback)));
01063 #else
01064         boost::shared_ptr<ServiceMessageHelper> *callback = (boost::shared_ptr<ServiceMessageHelper> *) cppCallback;
01065         AdvertiseServiceOptions aso(getString(env, name), *callback);
01066 #endif
01067 
01068         ServiceServer server = handle->advertiseService(aso);
01069         if (server) return (jlong) new ServiceServer(server);
01070         return 0;
01071 }
01072 
01073 JNIEXPORT jboolean JNICALL Java_ros_roscpp_JNI_isServiceServerValid
01074   (JNIEnv * env, jclass __jni, jlong cppServiceServer)
01075 {
01076         ServiceServer *server = (ServiceServer *) cppServiceServer;
01077         return (*server)  && ros::ok() ? true : false;
01078 }
01079 
01080 JNIEXPORT void JNICALL Java_ros_roscpp_JNI_shutdownServiceServer
01081   (JNIEnv * env, jclass __jni, jlong cppServiceServer)
01082 {
01083         ServiceServer *server = (ServiceServer *) cppServiceServer;
01084         delete server;
01085 }
01086 
01087 
01088 


rosjava_jni
Author(s): Maintained by Gheorghe Lisca (lisca@cs.uni-bremen.de) and developed by Jason Wolfe (jawolfe@willowgarage.com), Nicholas Butko (nbutko@cogsci.ucsd.edu), Lorenz Moesenlechner (moesenle@in.tum.de)
autogenerated on Sun Oct 5 2014 22:54:14