depth_camera.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
3 #include <ros/ros.h>
4 #include <sensor_msgs/PointCloud2.h>
5 
6 class DepthCameraTest : public testing::Test
7 {
8 protected:
9  virtual void SetUp()
10  {
11  has_new_image_ = false;
12  has_new_depth_ = false;
13  has_new_points_ = false;
14  }
15 
26 public:
27  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
28  {
29  image_stamp_ = msg->header.stamp;
30  has_new_image_ = true;
31  }
32  void depthCallback(const sensor_msgs::ImageConstPtr& msg)
33  {
34  depth_stamp_ = msg->header.stamp;
35  has_new_depth_ = true;
36  }
37  void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
38  {
39  points_stamp_ = msg->header.stamp;
40  has_new_points_ = true;
41  }
42 };
43 
44 // Test if the camera image is published at all, and that the timestamp
45 // is not too long in the past.
46 TEST_F(DepthCameraTest, cameraSubscribeTest)
47 {
49  cam_sub_ = it.subscribe("camera1/image_raw", 1,
51  dynamic_cast<DepthCameraTest*>(this));
52  depth_sub_ = it.subscribe("camera1/depth/image_raw", 1,
54  dynamic_cast<DepthCameraTest*>(this));
55  points_sub_ = nh_.subscribe("camera1/points", 1,
57  dynamic_cast<DepthCameraTest*>(this));
58 #if 0
59  // wait for gazebo to start publishing
60  // TODO(lucasw) this isn't really necessary since this test
61  // is purely passive
62  bool wait_for_topic = true;
63  while (wait_for_topic)
64  {
65  // @todo this fails without the additional 0.5 second sleep after the
66  // publisher comes online, which means on a slower or more heavily
67  // loaded system it may take longer than 0.5 seconds, and the test
68  // would hang until the timeout is reached and fail.
69  if (cam_sub_.getNumPublishers() > 0)
70  wait_for_topic = false;
71  ros::Duration(0.5).sleep();
72  }
73 #endif
74 
76  {
77  ros::spinOnce();
78  ros::Duration(0.1).sleep();
79  }
80 
81  EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec());
82  EXPECT_EQ(depth_stamp_.toSec(), points_stamp_.toSec());
83  // This check depends on the update period (currently 1.0/update_rate = 2.0 seconds)
84  // being much longer than the expected difference between now and the
85  // received image time.
86  const double max_time = 1.0;
87  const ros::Time current_time = ros::Time::now();
88  // TODO(lucasw)
89  // this likely isn't that robust - what if the testing system is really slow?
90  double time_diff;
91  time_diff = (current_time - image_stamp_).toSec();
92  ROS_INFO_STREAM(time_diff);
93  EXPECT_LT(time_diff, max_time);
94 
95  time_diff = (current_time - depth_stamp_).toSec();
96  ROS_INFO_STREAM(time_diff);
97  EXPECT_LT(time_diff, max_time);
98 
99  time_diff = (current_time - points_stamp_).toSec();
100  ROS_INFO_STREAM(time_diff);
101  EXPECT_LT(time_diff, max_time);
102 
103  cam_sub_.shutdown();
106 
107  // make sure nothing is subscribing to image_trigger topic
108  // there is no easy API, so call getSystemState
109  XmlRpc::XmlRpcValue args, result, payload;
110  args[0] = ros::this_node::getName();
111  EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true));
112  // [publishers, subscribers, services]
113  // subscribers in index 1 of payload
114  for (int i = 0; i < payload[1].size(); ++i)
115  {
116  // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
117  // topic name i is in index 0
118  std::string topic = payload[1][i][0];
119  EXPECT_EQ(topic.find("image_trigger"), std::string::npos);
120  }
121 }
122 
123 int main(int argc, char** argv)
124 {
125  ros::init(argc, argv, "gazebo_depth_camera_test");
126  testing::InitGoogleTest(&argc, argv);
127  return RUN_ALL_TESTS();
128 }
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())
image_transport::Subscriber cam_sub_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
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()
ros::Time depth_stamp_
ros::NodeHandle nh_
void depthCallback(const sensor_msgs::ImageConstPtr &msg)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
ros::Time points_stamp_
virtual void SetUp()
Definition: depth_camera.cpp:9
uint32_t getNumPublishers() const
#define ROS_INFO_STREAM(args)
void pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
TEST_F(DepthCameraTest, cameraSubscribeTest)
static Time now()
ros::Time image_stamp_
image_transport::Subscriber depth_sub_
ROSCPP_DECL void spinOnce()
ros::Subscriber points_sub_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
int main(int argc, char **argv)


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