triggered_camera.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
3 #include <ros/ros.h>
4 #include <std_msgs/Empty.h>
5 
6 class TriggeredCameraTest : public testing::Test
7 {
8 protected:
9  virtual void SetUp()
10  {
11  images_received_ = 0;
12  }
13 
18 public:
19  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
20  {
21  image_stamp_ = msg->header.stamp;
22  images_received_++;
23  }
24 };
25 
26 // Test if the camera image is published at all, and that the timestamp
27 // is not too long in the past.
28 TEST_F(TriggeredCameraTest, cameraSubscribeTest)
29 {
31  cam_sub_ = it.subscribe("camera1/image_raw", 5,
33  dynamic_cast<TriggeredCameraTest*>(this));
34 
35  // wait for 3 seconds to confirm that we don't receive any images
36  for (unsigned int i = 0; i < 30; ++i)
37  {
38  ros::spinOnce();
39  ros::Duration(0.1).sleep();
40  }
41  EXPECT_EQ(images_received_, 0);
42 
43  // make sure something is subscribing to image_trigger topic
44  // there is no easy API, so call getSystemState
45  XmlRpc::XmlRpcValue args, result, payload;
46  args[0] = ros::this_node::getName();
47  EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true));
48  // [publishers, subscribers, services]
49  // subscribers in index 1 of payload
50  int trigger_listeners = 0;
51  for (int i = 0; i < payload[1].size(); ++i)
52  {
53  // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
54  // topic name i is in index 0
55  std::string topic = payload[1][i][0];
56  if (topic.find("image_trigger") != std::string::npos)
57  {
58  trigger_listeners++;
59  }
60  }
61  EXPECT_EQ(trigger_listeners, 1);
62 
63  // publish to trigger topic and expect an update within one second:
64  ros::Publisher trigger_pub = nh_.advertise<std_msgs::Empty>("camera1/image_trigger", 1, true);
65  std_msgs::Empty msg;
66 
67  trigger_pub.publish(msg);
68  for (unsigned int i = 0; i < 10 && !images_received_; ++i)
69  {
70  ros::spinOnce();
71  ros::Duration(0.1).sleep();
72  }
73  EXPECT_EQ(images_received_, 1);
74 
75  // then wait for 3 seconds to confirm that we don't receive any more images
76  for (unsigned int i = 0; i < 30; ++i)
77  {
78  ros::spinOnce();
79  ros::Duration(0.1).sleep();
80  }
81  EXPECT_EQ(images_received_, 1);
82 
83  // then send two trigger messages very close together, and expect two more
84  // images
85  trigger_pub.publish(msg);
86  ros::spinOnce();
87  ros::Duration(0.01).sleep();
88  trigger_pub.publish(msg);
89  ros::spinOnce();
90  ros::Duration(0.01).sleep();
91  for (unsigned int i = 0; i < 10 && images_received_ < 2; ++i)
92  {
93  ros::spinOnce();
94  ros::Duration(0.1).sleep();
95  }
96  EXPECT_EQ(images_received_, 3);
97 
98  // then wait for 3 seconds to confirm that we don't receive any more images
99  for (unsigned int i = 0; i < 30; ++i)
100  {
101  ros::spinOnce();
102  ros::Duration(0.1).sleep();
103  }
104  EXPECT_EQ(images_received_, 3);
105 
106  cam_sub_.shutdown();
107 }
108 
109 int main(int argc, char** argv)
110 {
111  ros::init(argc, argv, "gazebo_camera_test");
112  testing::InitGoogleTest(&argc, argv);
113  return RUN_ALL_TESTS();
114 }
msg
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void publish(const boost::shared_ptr< M > &message) const
int size() const
virtual void SetUp()
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()
int main(int argc, char **argv)
image_transport::Subscriber cam_sub_
TEST_F(TriggeredCameraTest, cameraSubscribeTest)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
ROSCPP_DECL void spinOnce()


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39