Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include <time.h>
00015
00016 #include <glog/logging.h>
00017 #include <gtest/gtest.h>
00018 #include <image_transport/publisher_plugin.h>
00019 #include <nodelet/loader.h>
00020 #include <pluginlib/class_loader.h>
00021 #include <ros/ros.h>
00022 #include <tango_ros_messages/TangoConnect.h>
00023
00024 std::string master_uri;
00025 std::string host_ip;
00026
00027 class TangoRosTest : public ::testing::Test {
00028 public:
00029 constexpr static int TEST_DURATION = 20;
00030 };
00031
00032 TEST_F(TangoRosTest, TestPublishingForFixedTime) {
00033 int argc = 3;
00034 char* argv[] = {"/", &master_uri[0], &host_ip[0]};
00035 ros::init(argc, argv, "tango");
00036
00037 nodelet::Loader loader;
00038 std::map<std::string, std::string> remappings;
00039 std::vector<std::string> nodelet_argv;
00040 LOG(INFO) << "Start loading nodelets.";
00041 const bool result = loader.load("/tango", "tango_ros_native/TangoRosNodelet", remappings, nodelet_argv);
00042 ASSERT_TRUE(result) << "Problem loading Tango ROS nodelet!";
00043 LOG(INFO) << "Finished loading nodelets.";
00044
00045
00046 pluginlib::ClassLoader<image_transport::PublisherPlugin> image_transport_pub_loader("image_transport", "image_transport::PublisherPlugin");
00047 ASSERT_TRUE(image_transport_pub_loader.isClassAvailable("image_transport/raw_pub")) << "Plugin image_transport/raw_pub is not available.";
00048 ASSERT_TRUE(image_transport_pub_loader.isClassAvailable("image_transport/compressed_pub")) << "Plugin image_transport/compressed_pub is not available.";
00049
00050 ros::NodeHandle nh;
00051 ros::ServiceClient tango_connect_service = nh.serviceClient<tango_ros_messages::TangoConnect>("/tango/connect");
00052 tango_ros_messages::TangoConnect srv;
00053 srv.request.request = tango_ros_messages::TangoConnect::Request::CONNECT;
00054 ASSERT_TRUE(tango_connect_service.call(srv)) << "Call to tango connect failed.";
00055
00056 time_t current_time = time(NULL);
00057 time_t end = current_time + TEST_DURATION;
00058 while(current_time < end) {
00059 current_time = time(NULL);
00060 }
00061 }
00062
00063
00064 int main(int argc, char **argv) {
00065 testing::InitGoogleTest(&argc, argv);
00066 master_uri = argv[1];
00067 host_ip = argv[2];
00068 return RUN_ALL_TESTS();
00069 }