test_rosjava_jni: test_rosjava_jni: Testee.java Source File

00001 
00002 import ros.*;
00003 import ros.communication.*;
00004 import ros.pkg.test_rosjava_jni.msg.*;
00005 import ros.pkg.test_rosjava_jni.srv.*;
00006 import ros.pkg.std_msgs.msg.ByteMultiArray;
00007 import ros.pkg.std_msgs.msg.MultiArrayLayout;
00008 import ros.pkg.std_msgs.msg.MultiArrayDimension;
00009 
00010 class Testee {
00011         public static void main(String [] args) throws InterruptedException, RosException{
00012                 System.out.println("Starting rosjava.");
00013                 Ros ros = Ros.getInstance();
00014                 ros.init("testNode");
00015                 
00016                 // TODO: test these somehow?
00017                 ros.logDebug("DEBUG");
00018                 ros.logInfo("INFO");
00019                 ros.logWarn("WARN");
00020                 ros.logError("ERROR");
00021                 ros.logFatal("FATAL");
00022                 
00023                 System.out.println("Initialized");              
00024                 NodeHandle n = ros.createNodeHandle();
00025 
00026                 // Can't call Time::now() until *after* the above node
00027                 // handle has been created.
00028                 System.out.println(ros.now());
00029                 
00030                 ros.pkg.std_msgs.msg.String msg = new ros.pkg.std_msgs.msg.String();
00031                 msg.data = "go";
00032                 Publisher<ros.pkg.std_msgs.msg.String> pub = n.advertise("/talk", msg, 1);
00033                 
00034                 Publisher<TestDataTypes> pub2 = n.advertise("/test_talk", new TestDataTypes(), 1);
00035                 
00036                 Publisher<TestBadDataTypes> pub3 = n.advertise("/test_bad", new TestBadDataTypes(), 1);
00037                 
00038                 
00039                 Subscriber.QueueingCallback<ros.pkg.std_msgs.msg.String> cb = new Subscriber.QueueingCallback<ros.pkg.std_msgs.msg.String>(); 
00040                 Subscriber<ros.pkg.std_msgs.msg.String> sub = n.subscribe("/listen", new ros.pkg.std_msgs.msg.String(), cb, 10);
00041                 
00042                 Subscriber.QueueingCallback<TestDataTypes> cb2 = new Subscriber.QueueingCallback<TestDataTypes>(); 
00043                 Subscriber<TestDataTypes> sub2 = n.subscribe("/test_listen", new TestDataTypes(), cb2, 10);
00044                 
00045                 Subscriber.QueueingCallback<TestBadDataTypes> cb3 = new Subscriber.QueueingCallback<TestBadDataTypes>(); 
00046                 Subscriber<TestBadDataTypes> sub3 = n.subscribe("/test_bad", new TestBadDataTypes(), cb3, 10);
00047                 
00048                 System.out.println("Waiting for roscpp...");
00049                 for(int i = 0; i < 60 && cb.size() == 0; i++) {
00050                         pub.publish(msg);
00051                         n.spinOnce();
00052                         Thread.sleep(1000);
00053                 }
00054                 System.out.println("Started");
00055                 
00057                 msg.data = "good";
00058                 if (n.getIntParam("int_param") != 1) msg.data="fail_int_param";
00059                 n.setParam("int_param2", n.getIntParam("int_param"));
00060                 if (n.getDoubleParam("double_param") != 1.0) msg.data="fail_double_param";
00061                 n.setParam("double_param2", n.getDoubleParam("double_param"));
00062                 if (n.getStringParam("string_param") == "hello") msg.data = "fail_string_param";
00063                 n.setParam("string_param2", n.getStringParam("string_param"));
00064                 
00065 
00066                 // Test services
00067                 ServiceServer.Callback<TestTwoInts.Request,TestTwoInts.Response> scb = 
00068                         new ServiceServer.Callback<TestTwoInts.Request,TestTwoInts.Response>() {
00069                                 public TestTwoInts.Response call(TestTwoInts.Request request) {
00070                                         TestTwoInts.Response res = new TestTwoInts.Response();
00071                                         res.sum = request.a + request.b;
00072                                         return res;
00073                                 }
00074                         };                      
00075                         
00076                 ServiceServer<TestTwoInts.Request,TestTwoInts.Response,TestTwoInts> srv = n.advertiseService("add_two_ints_java", new TestTwoInts(), scb);
00077                 
00078                 pub.publish(msg);
00079 
00080                 ServiceClient<TestTwoInts.Request, TestTwoInts.Response, TestTwoInts> sc = n.serviceClient("add_two_ints_cpp" , new TestTwoInts(), false);
00081                 TestTwoInts.Request rq = new TestTwoInts.Request();
00082                 rq.a = 12;
00083                 rq.b = 17;
00084                 msg.data="good";
00085                 if (sc.call(rq).sum != 29) {
00086                         System.out.println("Got incorrect sum!");
00087                         msg.data="bad";
00088                 }
00089                 pub.publish(msg);
00090 
00091                 // Test messages
00092                 while(cb2.size() == 0) {
00093                         n.spinOnce();
00094                         Thread.sleep(1000);
00095                 }
00096 
00097                 msg.data="good";
00098                 TestDataTypes test = cb2.pop();
00099                 if ((0xff&(int)test.byte_) != 0xab)   msg.data="fail_byte";
00100                 if ((0xff&(int)test.char_) != 0xbc)   msg.data="fail_char";
00101                 if ((0xff&(int)test.uint8_) != 0xcd)  msg.data="fail_ui8";
00102                 if ((0xff&(int)test.int8_) != 0xde)    msg.data="fail_i8";
00103                 if ((0xffff&(int)test.uint16_) != 0xabcd) msg.data="fail_ui16";
00104                 if ((0xffff&(int)test.int16_) != 0xbcde)   msg.data="fail_i16:" + test.int16_;
00105                 if (test.uint32_ != 0xdeadbeef) msg.data="fail_ui32";
00106                 if (test.int32_ != 0xcabcabbe)   msg.data="fail_i32";
00107                 if (test.uint64_ != 0xbeefcabedeaddeedL) msg.data="fail_ui64";
00108                 if (test.int64_ != 0xfeedbabedeadbeefL)   msg.data="fail_i64";
00109                 if (test.float32_ != 1.0)   msg.data="fail_f32";
00110                 if (test.float64_ != -1.0)  msg.data="fail_f64";
00111                 if (!test.string_.equals("hello")) msg.data="fail_string";
00112                 if (test.time_.secs != 123) msg.data="fail_time";
00113                 if (test.time_.nsecs != 456) msg.data="fail_time";
00114                 if (test.duration_.secs != 789) msg.data="fail_duration";
00115                 if (test.duration_.nsecs != 987) msg.data="fail_duration";
00116                 
00117                 if (test.byte_v.length != 1) msg.data="fail_byte_v_len";
00118                 if (test.byte_v[0] != 11) msg.data="fail_byte_v[0]";
00119                 if (test.byte_f[0] != 22) msg.data="fail_byte_f[0]";
00120                 if (test.byte_f[1] != 33) msg.data="fail_byte_f[1]";
00121                 
00122                 if (test.float64_v.length != 1) msg.data="fail_float64_v_len";
00123                 if (test.float64_v[0] != 1.0) msg.data="fail_float64_v[0]";
00124                 if (test.float64_f[0] != 2.0) msg.data="fail_float64_f[0]";
00125                 if (test.float64_f[1] != 3.0) msg.data="fail_float64_f[1]";
00126                 
00127                 if (test.string_v.size() != 1) msg.data="fail_string_v_len";
00128                 if (!test.string_v.get(0).equals("test1")) msg.data="fail_string_v[0]";
00129                 if (!test.string_f[0].equals("")) msg.data="fail_string_f[0]";
00130                 if (!test.string_f[1].equals("test3")) msg.data="fail_string_f[1]";
00131                 
00132                 if (test.time_v.size() != 1) msg.data="fail_time_v_len";
00133                 if (test.time_v.get(0).secs != 222) msg.data="fail_time_v[0]";
00134                 if (test.time_f[0].secs != 444) msg.data="fail_time_f[0]";
00135                 if (test.time_f[1].secs != 666) msg.data="fail_time_f[1]";
00136                 if (test.time_v.get(0).nsecs != 333) msg.data="fail_time_v[0]";
00137                 if (test.time_f[0].nsecs != 555) msg.data="fail_time_f[0]";
00138                 if (test.time_f[1].nsecs != 777) msg.data="fail_time_f[1]";
00139                 
00140                 if (test.Byte_.data != 1) msg.data="fail_Byte_";
00141                 if (test.Byte_v.size() != 2) msg.data="fail_Byte_v_length";
00142                 if (test.Byte_v.get(0).data != (byte)2) msg.data="fail_Byte_v[0]";
00143                 if (test.Byte_v.get(1).data != (byte)3) msg.data="fail_Byte_v[1]";
00144 
00145                 if (test.ByteMultiArray_.layout.dim.size() != 1) msg.data="fail_ByteMultiArray_layout_dims";
00146                 if (!test.ByteMultiArray_.layout.dim.get(0).label.equals("test")) msg.data="fail_ByteMultiArray_layout_dim[0]_label";
00147                 if (test.ByteMultiArray_.layout.dim.get(0).size != 1) msg.data="fail_ByteMultiArray_layout_dim[0]_size";
00148                 if (test.ByteMultiArray_.layout.dim.get(0).stride != 1) msg.data="fail_ByteMultiArray_layout_dim[0]_stride";
00149                 if (test.ByteMultiArray_.layout.data_offset != 0) msg.data="fail_ByteMultiArray_layout_data_offset";
00150                 if (test.ByteMultiArray_.data.length != 1) msg.data="fail_ByteMultiArray_data_length";
00151                 if (test.ByteMultiArray_.data[0] != (byte)11) msg.data="fail_ByteMultiArray_data[0]";
00152 
00153                 if (test.ByteMultiArray_v.size() != 1) msg.data="fail_ByteMultiArray_v_length";
00154                 if (test.ByteMultiArray_v.get(0).layout.dim.size() != 1) msg.data="fail_ByteMultiArray_v[0]layout_dims";
00155                 if (!test.ByteMultiArray_v.get(0).layout.dim.get(0).label.equals("test")) msg.data="fail_ByteMultiArray_v.get(0)layout_dim.get(0)_label:" + test.ByteMultiArray_v.get(0).layout.dim.get(0).label;
00156                 if (test.ByteMultiArray_v.get(0).layout.dim.get(0).size != 1) msg.data="fail_ByteMultiArray_v.get(0)layout_dim.get(0)_size";
00157                 if (test.ByteMultiArray_v.get(0).layout.dim.get(0).stride != 1) msg.data="fail_ByteMultiArray_v.get(0)layout_dim.get(0)_stride";
00158                 if (test.ByteMultiArray_v.get(0).layout.data_offset != 0) msg.data="fail_ByteMultiArray_v.get(0)layout_data_offset";
00159                 if (test.ByteMultiArray_v.get(0).data.length != 1) msg.data="fail_ByteMultiArray_v.get(0)data_length";
00160                 if (test.ByteMultiArray_v.get(0).data[0] != (byte)11) msg.data="fail_ByteMultiArray_v.get(0)data.get(0)";
00161 
00162 //              TestDataTypes tmp_ = new TestDataTypes();
00163 //              tmp_.deserialize(test.serialize(0));
00164                 
00165                 System.out.println("Result of good msg test: " + msg.data);
00166                 pub2.publish(test);
00167                 
00168                 // Now, test the types that are still not built correctly by roscpp
00169                 TestBadDataTypes tbdt = new TestBadDataTypes();
00170                 tbdt.Byte_f[0].data = 0xfe;
00171                 tbdt.Byte_f[1].data = 0xcd;
00172                 tbdt.ByteMultiArray_f[0].layout.dim.add(new MultiArrayDimension());
00173                 tbdt.ByteMultiArray_f[0].layout.dim.get(0).label="test";
00174                 tbdt.ByteMultiArray_f[0].layout.dim.get(0).size=2;
00175                 tbdt.ByteMultiArray_f[0].layout.dim.get(0).stride=1;
00176     tbdt.ByteMultiArray_f[0].layout.data_offset=0;
00177     tbdt.ByteMultiArray_f[0].data = new short[2];
00178                 tbdt.ByteMultiArray_f[0].data[0] = (short)0xab;
00179                 tbdt.ByteMultiArray_f[0].data[1] = (short)0xdc;
00180 
00181                 // Ensure we serialize and deserialize, in case roscpp does something fancy under the hood (i.e., direct transit)
00182                 TestBadDataTypes temp = new TestBadDataTypes();
00183                 temp.deserialize(tbdt.serialize(0)); 
00184                 pub3.publish(temp);
00185 
00186 //              pub3.publish(tbdt);
00187                 
00188                 while(cb3.size() == 0) {
00189                         n.spinOnce();
00190                 }
00191                 tbdt = cb3.pop();
00192                 if (tbdt.Byte_f.length != 2) msg.data="fail_Byte_f_len";
00193                 if (tbdt.Byte_f[0].data != 0xfe) msg.data="fail_Byte_f[0]";
00194                 if (tbdt.Byte_f[1].data != 0xcd) msg.data="fail_Byte_f[1]";
00195                 if (tbdt.ByteMultiArray_f.length != 1) msg.data="fail_ByteMultiArray_f_length";
00196                 if (tbdt.ByteMultiArray_f[0].layout.dim.size() != 1) msg.data="fail_ByteMultiArray_f_dims";
00197                 if (!tbdt.ByteMultiArray_f[0].layout.dim.get(0).label.equals("test")) msg.data="fail_ByteMultiArray_f_dim[0]_label";
00198                 if (tbdt.ByteMultiArray_f[0].layout.dim.get(0).size != 2) msg.data="fail_ByteMultiArray_f_dim[0]_size";
00199                 if (tbdt.ByteMultiArray_f[0].layout.dim.get(0).stride != 1) msg.data="fail_ByteMultiArray_f_dim[0]_stride";
00200                 if (tbdt.ByteMultiArray_f[0].layout.data_offset != 0) msg.data="fail_ByteMultiArray_f_data_offset";
00201                 if (tbdt.ByteMultiArray_f[0].data.length != 2) msg.data="fail_ByteMultiArray_f_data_length";
00202                 if (tbdt.ByteMultiArray_f[0].data[0] != 0xab) msg.data="fail_ByteMultiArray_f_data[0]";
00203                 if (tbdt.ByteMultiArray_f[0].data[1] != 0xdc) msg.data="fail_ByteMultiArray_f_data[1]";
00204                 
00205                 System.out.println("Result of bad msg test: " + msg.data);
00206                 
00207 
00208                 pub.publish(msg);
00209                 
00210         
00211                 /*      
00212 
00213 
00214                         
00215 
00216                         Subscriber.QueueingCallback<ros.pkg.rosjava_test.msg.String> cb = new Subscriber.QueueingCallback<ros.pkg.rosjava_test.msg.String>(); 
00217                         Subscriber<ros.pkg.rosjava_test.msg.String> sub = n.subscribe("/chatter", new ros.pkg.rosjava_test.msg.String(), cb, 10);
00218                         Thread.sleep(100);
00219                         
00220                         System.out.print("Published topics: ");
00221                         for (Topic top : n.getTopics()) {
00222                                 System.out.print(top.getName() + ":" + top.getDatatype() + ", ");
00223                         }
00224                         System.out.println();
00225 
00226                         System.out.print("Advertised topics: ");
00227                         for (String s : n.getAdvertisedTopics()) {
00228                                 System.out.print(s  + ", ");
00229                         }
00230                         System.out.println();
00231 
00232                         System.out.print("Subscribed topics: ");
00233                         for (String s : n.getSubscribedTopics()) {
00234                                 System.out.print(s  + ", ");
00235                         }
00236                         System.out.println();
00237 
00238                         
00239                         for(int i = 0; i < 50; i++) {
00240                                 System.out.println(i);
00241                                 ros.pkg.rosjava_test.msg.String m = new ros.pkg.rosjava_test.msg.String();
00242                                 m.data = "Hola " + i;
00243                                 pub.publish(m);
00244                                 if (i == 37) sub.shutdown();
00245 
00246                                 while (!cb.isEmpty()) {
00247                                         System.out.println(cb.pop().data);
00248                                 }
00249                                 Thread.sleep(100);
00250                                 ros.spinOnce();
00251                         }
00252                         
00253                         pub.shutdown();
00254                         srv.shutdown();
00255                         sc.shutdown();
00256 
00257                         n.shutdown();
00258                 } 
00259                 */              
00260         }
00261 }