test_rosjava_jni: test_rosjava_jni: tester.cpp Source File

00001 /*
00002  * Copyright (C) 2008, Jason Wolfe and Willow Garage, Inc.
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *   * Redistributions of source code must retain the above copyright notice,
00007  *     this list of conditions and the following disclaimer.
00008  *   * Redistributions in binary form must reproduce the above copyright
00009  *     notice, this list of conditions and the following disclaimer in the
00010  *     documentation and/or other materials provided with the distribution.
00011  *   * Neither the name of Willow Garage, Inc. nor the names of its
00012  *     contributors may be used to endorse or promote products derived from
00013  *     this software without specific prior written permission.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 #include "ros/ros.h"
00029 #include "test_rosjava_jni/TestDataTypes.h"
00030 #include "test_rosjava_jni/TestTwoInts.h"
00031 #include "std_msgs/String.h"
00032 #include <gtest/gtest.h>
00033 #include <ros/time.h>
00034 #include <ros/console.h>
00035 
00036 using namespace test_rosjava_jni;
00037 
00038 int num_messages_received=0;
00039 std::string last_message = "";
00040 TestDataTypes test_in;
00041 
00042 void chatterCallback(const std_msgs::StringConstPtr& msg)
00043 {
00044   num_messages_received++;
00045   last_message = msg->data;
00046 }
00047 
00048 void tdtCallback(const boost::shared_ptr<const TestDataTypes>& msg)
00049 {
00050   test_in = *msg;
00051 }
00052 
00053 bool add(TestTwoInts::Request  &req,
00054          TestTwoInts::Response &res )
00055 {
00056   res.sum = req.a + req.b;
00057   ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
00058   ROS_INFO("  sending back response: [%ld]", (long int)res.sum);
00059   return true;
00060 }
00061 
00062 
00063 
00064 TEST(Rosjava, rosjava)
00065 {
00066   ros::NodeHandle n;
00067   ros::Subscriber sub = n.subscribe("talk", 1000, chatterCallback);
00068   ros::Subscriber sub2 = n.subscribe("test_talk", 1000, tdtCallback);
00069   ros::Duration d(1);
00070 
00071   ros::ServiceServer server = n.advertiseService("add_two_ints_cpp", add);
00072 
00073   int wait_max=180;
00074 
00075   for (int i=0; i<wait_max && n.ok(); i++) {
00076     d.sleep();
00077     ros::spinOnce();
00078     ROS_INFO_STREAM ("Have waited " << i << " out of " << wait_max << " seconds to receive a message from chatter-echo");
00079     if (num_messages_received>0)
00080       break;
00081   }
00082   EXPECT_TRUE(num_messages_received>0);
00083   ROS_INFO_STREAM ("Successfully received a startup message.");
00084 
00085   ros::Publisher pub = n.advertise<std_msgs::String>("listen", 10);
00086   ros::Publisher pub2 = n.advertise<TestDataTypes>("test_listen", 1);
00087   std_msgs::String msg;
00088   msg.data = "Hi";
00089 
00090   n.setParam("int_param", 1);
00091   n.setParam("double_param", 1.0);
00092   n.setParam("string_param", "hello");
00093 
00094   int i = 0;
00095   while (pub.getNumSubscribers() < 1 && i++ < wait_max) {
00096           ros::spinOnce();
00097           d.sleep();
00098           ROS_INFO_STREAM("Waiting for subscriber...");
00099   }
00100 
00101   EXPECT_TRUE(pub.getNumSubscribers() == 1);
00102   pub.publish(msg);
00103 
00104   d.sleep();
00105   ros::spinOnce();
00106   num_messages_received=0;
00107   while(num_messages_received == 0 && i++ < wait_max) {
00108           ros::spinOnce();
00109           d.sleep();
00110   }
00111   EXPECT_EQ(last_message, "good"); // Result for testint params in Java
00112   ROS_INFO_STREAM("Successfully tested reading parameters");
00113 
00114   // Test parameters
00115   int ip; double dp; std::string sp;
00116   n.getParam("int_param2", ip);
00117   n.getParam("double_param2", dp);
00118   n.getParam("string_param2", sp);
00119   EXPECT_EQ(ip, 1);
00120   EXPECT_EQ(dp, 1.0);
00121   EXPECT_EQ(sp, "hello");
00122 
00123   ROS_INFO_STREAM("Successfully tested writing parameters");
00124 
00125   // Test service client
00126   TestTwoInts::Request req;
00127   TestTwoInts::Response res;
00128   req.a = 7; req.b = 4;
00129   ros::ServiceClient service = n.serviceClient<TestTwoInts>("add_two_ints_java");
00130   EXPECT_TRUE(service.call(req, res));
00131   EXPECT_EQ(res.sum, 11);
00132 
00133   service = n.serviceClient<TestTwoInts>("add_two_ints_java", true);
00134   req.b = 5;
00135   EXPECT_TRUE(service.call(req, res));
00136   EXPECT_EQ(res.sum, 12);
00137   req.a = 8;
00138   EXPECT_TRUE(service.call(req, res));
00139   EXPECT_EQ(res.sum, 13);
00140 
00141   num_messages_received=0;
00142   while(num_messages_received == 0 && i++ < wait_max) {
00143           ros::spinOnce();
00144           d.sleep();
00145   }
00146   EXPECT_EQ(last_message, "good"); // Result for calling C++ service from java.
00147 
00148   ROS_INFO_STREAM("Successfully tested services");
00149 
00150   TestDataTypes test;
00151   test.byte_ = 0xab;
00152   test.char_ = 0xbc;
00153   test.uint8_ = 0xcd;
00154   test.int8_  = 0xde;
00155   test.uint16_ = 0xabcd;
00156   test.int16_  = 0xbcde;
00157   test.uint32_ = 0xdeadbeef;
00158   test.int32_  = 0xcabcabbe;
00159   test.uint64_ = 0xbeefcabedeaddeedLL;
00160   test.int64_  = 0xfeedbabedeadbeefLL;
00161   test.float32_ = 1.0;
00162   test.float64_  = -1.0;
00163   test.string_ = "hello";
00164   test.time_ = ros::Time(123,456);
00165   test.duration_ = ros::Duration(789,987);
00166 
00167   test.byte_v.push_back(11);
00168   test.byte_f[0] = 22;
00169   test.byte_f[1] = 33;
00170 
00171   test.float64_v.push_back(1.0);
00172   test.float64_f[0] = 2.0;
00173   test.float64_f[1] = 3.0;
00174 
00175   test.string_v.push_back("test1");
00176   test.string_f[0] = "";
00177   test.string_f[1] = "test3";
00178 
00179   test.time_v.push_back(ros::Time(222,333));
00180   test.time_f[0] = ros::Time(444,555);
00181   test.time_f[1] = ros::Time(666,777);
00182 
00183   test.Byte_.data = 1;
00184   std_msgs::Byte tmp;
00185   tmp.data = 2;
00186   test.Byte_v.push_back(tmp);
00187   tmp.data = 3;
00188   test.Byte_v.push_back(tmp);
00189 
00190   std_msgs::ByteMultiArray tmp2;
00191   std_msgs::MultiArrayDimension tmp3;
00192   tmp3.label="test";
00193   tmp3.size=1;
00194   tmp3.stride=1;
00195   tmp2.layout.dim.push_back(tmp3);
00196   tmp2.layout.data_offset=0;
00197   tmp2.data.push_back(11);
00198 
00199   test.ByteMultiArray_ = tmp2;
00200   test.ByteMultiArray_v.push_back(tmp2);
00201 
00202   pub2.publish(test);
00203 
00204   num_messages_received=0;
00205   while(num_messages_received == 0 && i++ < wait_max) {
00206           ros::spinOnce();
00207           d.sleep();
00208   }
00209   EXPECT_EQ(num_messages_received, 1);
00210   EXPECT_EQ(last_message, "good"); // Result for reading message in Java.
00211 
00212   // Now, test we got things back as expected.
00213   EXPECT_EQ(test_in.byte_, test.byte_);
00214   EXPECT_EQ(test_in.char_, test.char_);
00215   EXPECT_EQ(test_in.uint8_, test.uint8_);
00216   EXPECT_EQ(test_in.int8_, test.int8_);
00217   EXPECT_EQ(test_in.uint16_, test.uint16_);
00218   EXPECT_EQ(test_in.int16_, test.int16_);
00219   EXPECT_EQ(test_in.uint32_, test.uint32_);
00220   EXPECT_EQ(test_in.int32_, test.int32_);
00221   EXPECT_EQ(test_in.uint64_, test.uint64_);
00222   EXPECT_EQ(test_in.int64_, test.int64_);
00223   EXPECT_EQ(test_in.float32_, test.float32_);
00224   EXPECT_EQ(test_in.float64_, test.float64_);
00225   EXPECT_EQ(test_in.string_, test.string_);
00226   EXPECT_EQ(test_in.time_, test.time_);
00227   EXPECT_EQ(test_in.duration_, test.duration_);
00228 
00229   EXPECT_EQ(test_in.byte_v.size(), test.byte_v.size());
00230   EXPECT_EQ(test_in.byte_v[0], test.byte_v[0]);
00231   EXPECT_EQ(test_in.byte_f[0], test.byte_f[0]);
00232   EXPECT_EQ(test_in.byte_f[1], test.byte_f[1]);
00233 
00234   EXPECT_EQ(test_in.float64_v.size(), test.float64_v.size());
00235   EXPECT_EQ(test_in.float64_v[0], test.float64_v[0]);
00236   EXPECT_EQ(test_in.float64_f[0], test.float64_f[0]);
00237   EXPECT_EQ(test_in.float64_f[1], test.float64_f[1]);
00238 
00239   EXPECT_EQ(test_in.string_v.size(), test.string_v.size());
00240   EXPECT_EQ(test_in.string_v[0], test.string_v[0]);
00241   EXPECT_EQ(test_in.string_f[0], test.string_f[0]);
00242   EXPECT_EQ(test_in.string_f[1], test.string_f[1]);
00243 
00244   EXPECT_EQ(test_in.time_v.size(), test.time_v.size());
00245   EXPECT_EQ(test_in.time_v[0], test.time_v[0]);
00246   EXPECT_EQ(test_in.time_f[0], test.time_f[0]);
00247   EXPECT_EQ(test_in.time_f[1], test.time_f[1]);
00248 
00249   EXPECT_EQ(test_in.Byte_.data, test.Byte_.data);
00250   EXPECT_EQ(test_in.Byte_v.size(), test.Byte_v.size());
00251   EXPECT_EQ(test_in.Byte_v[0].data, test.Byte_v[0].data);
00252   EXPECT_EQ(test_in.Byte_v[1].data, test.Byte_v[1].data);
00253 
00254   EXPECT_EQ(test_in.ByteMultiArray_.layout.dim.size(), test.ByteMultiArray_.layout.dim.size());
00255   EXPECT_EQ(test_in.ByteMultiArray_.layout.dim[0].label, test.ByteMultiArray_.layout.dim[0].label);
00256   EXPECT_EQ(test_in.ByteMultiArray_.layout.dim[0].size, test.ByteMultiArray_.layout.dim[0].size);
00257   EXPECT_EQ(test_in.ByteMultiArray_.layout.dim[0].stride, test.ByteMultiArray_.layout.dim[0].stride);
00258   EXPECT_EQ(test_in.ByteMultiArray_.layout.data_offset, test.ByteMultiArray_.layout.data_offset);
00259   EXPECT_EQ(test_in.ByteMultiArray_.data.size(), test.ByteMultiArray_.data.size());
00260   EXPECT_EQ(test_in.ByteMultiArray_.data[0], test.ByteMultiArray_.data[0]);
00261 
00262   EXPECT_EQ(test_in.ByteMultiArray_v.size(), test.ByteMultiArray_v.size());
00263   EXPECT_EQ(test_in.ByteMultiArray_v[0].layout.dim.size(), test.ByteMultiArray_v[0].layout.dim.size());
00264   EXPECT_EQ(test_in.ByteMultiArray_v[0].layout.dim[0].label, test.ByteMultiArray_v[0].layout.dim[0].label);
00265   EXPECT_EQ(test_in.ByteMultiArray_v[0].layout.dim[0].size, test.ByteMultiArray_v[0].layout.dim[0].size);
00266   EXPECT_EQ(test_in.ByteMultiArray_v[0].layout.dim[0].stride, test.ByteMultiArray_v[0].layout.dim[0].stride);
00267   EXPECT_EQ(test_in.ByteMultiArray_v[0].layout.data_offset, test.ByteMultiArray_v[0].layout.data_offset);
00268   EXPECT_EQ(test_in.ByteMultiArray_v[0].data.size(), test.ByteMultiArray_v[0].data.size());
00269   EXPECT_EQ(test_in.ByteMultiArray_v[0].data[0], test.ByteMultiArray_v[0].data[0]);
00270 
00271   ROS_INFO_STREAM("Successfully tested messages");
00272 
00273 }
00274 
00275 
00276 
00277 int main(int argc, char **argv)
00278 {
00279   testing::InitGoogleTest(&argc, argv);
00280   ros::init(argc, argv, "tester");
00281 
00282 //  return -1;
00283   return RUN_ALL_TESTS();
00284 }