multicamera.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 // #include <image_transport/image_transport.h>
3 // #include <image_transport/subscriber_filter.h>
4 // #include <message_filters/sync_policies/approximate_time.h>
7 #include <ros/ros.h>
8 #include <sensor_msgs/Image.h>
9 
10 class MultiCameraTest : public testing::Test
11 {
12 protected:
13  virtual void SetUp()
14  {
15  has_new_image_ = false;
16  }
17 
19  // image_transport::SubscriberFilter cam_left_sub_;
20  // image_transport::SubscriberFilter cam_right_sub_;
23 
27 
28  // typedef message_filters::sync_policies::ApproximateTime<
29  // sensor_msgs::Image, sensor_msgs::Image
30  // > MySyncPolicy;
31  // message_filters::Synchronizer< MySyncPolicy > sync_;
32 
33 
34 public:
36  const sensor_msgs::ImageConstPtr& left_msg,
37  const sensor_msgs::ImageConstPtr& right_msg)
38  {
39  image_left_stamp_ = left_msg->header.stamp;
40  image_right_stamp_ = right_msg->header.stamp;
41  has_new_image_ = true;
42  }
43 };
44 
45 // Test if the camera image is published at all, and that the timestamp
46 // is not too long in the past.
47 TEST_F(MultiCameraTest, cameraSubscribeTest)
48 {
49  // image_transport::ImageTransport it(nh_);
50  // cam_left_sub_.subscribe(it, "stereo/camera/left/image_raw", 1);
51  // cam_right_sub_.subscribe(it, "stereo/camera/right/image_raw", 1);
52  // sync_ = message_filters::Synchronizer<MySyncPolicy>(
53  // MySyncPolicy(4), cam_left_sub_, cam_right_sub_);
54  cam_left_sub_.subscribe(nh_, "stereo/camera/left/image_raw", 1);
55  cam_right_sub_.subscribe(nh_, "stereo/camera/right/image_raw", 1);
59  dynamic_cast<MultiCameraTest*>(this), _1, _2));
60 #if 0
61  // wait for gazebo to start publishing
62  // TODO(lucasw) this isn't really necessary since this test
63  // is purely passive
64  bool wait_for_topic = true;
65  while (wait_for_topic)
66  {
67  // @todo this fails without the additional 0.5 second sleep after the
68  // publisher comes online, which means on a slower or more heavily
69  // loaded system it may take longer than 0.5 seconds, and the test
70  // would hang until the timeout is reached and fail.
71  if (cam_sub_.getNumPublishers() > 0)
72  wait_for_topic = false;
73  ros::Duration(0.5).sleep();
74  }
75 #endif
76 
77  while (!has_new_image_)
78  {
79  ros::spinOnce();
80  ros::Duration(0.1).sleep();
81  }
82 
83  double sync_diff = (image_left_stamp_ - image_right_stamp_).toSec();
84  EXPECT_EQ(sync_diff, 0.0);
85 
86  // This check depends on the update period being much longer
87  // than the expected difference between now and the received image time
88  // TODO(lucasw)
89  // this likely isn't that robust - what if the testing system is really slow?
90  double time_diff = (ros::Time::now() - image_left_stamp_).toSec();
91  ROS_INFO_STREAM(time_diff);
92  EXPECT_LT(time_diff, 1.0);
93  // cam_sub_.shutdown();
94 
95  // make sure nothing is subscribing to image_trigger topic
96  // there is no easy API, so call getSystemState
97  XmlRpc::XmlRpcValue args, result, payload;
98  args[0] = ros::this_node::getName();
99  EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true));
100  // [publishers, subscribers, services]
101  // subscribers in index 1 of payload
102  for (int i = 0; i < payload[1].size(); ++i)
103  {
104  // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
105  // topic name i is in index 0
106  std::string topic = payload[1][i][0];
107  EXPECT_EQ(topic.find("image_trigger"), std::string::npos);
108  }
109 }
110 
111 int main(int argc, char** argv)
112 {
113  ros::init(argc, argv, "gazebo_multicamera_test");
114  testing::InitGoogleTest(&argc, argv);
115  return RUN_ALL_TESTS();
116 }
ros::Time image_right_stamp_
Definition: multicamera.cpp:26
message_filters::Subscriber< sensor_msgs::Image > cam_left_sub_
Definition: multicamera.cpp:21
int size() const
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
virtual void SetUp()
Definition: multicamera.cpp:13
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
ros::Time image_left_stamp_
Definition: multicamera.cpp:25
TEST_F(MultiCameraTest, cameraSubscribeTest)
Definition: multicamera.cpp:47
int main(int argc, char **argv)
ros::NodeHandle nh_
Definition: multicamera.cpp:18
#define ROS_INFO_STREAM(args)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static Time now()
void imageCallback(const sensor_msgs::ImageConstPtr &left_msg, const sensor_msgs::ImageConstPtr &right_msg)
Definition: multicamera.cpp:35
ROSCPP_DECL void spinOnce()
message_filters::Subscriber< sensor_msgs::Image > cam_right_sub_
Definition: multicamera.cpp:22


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27