00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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");
00112 ROS_INFO_STREAM("Successfully tested reading parameters");
00113
00114
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
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");
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");
00211
00212
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
00283 return RUN_ALL_TESTS();
00284 }