test_assembler.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Vijay Pradeep */
36 
37 #include <string>
38 #include <gtest/gtest.h>
39 #include <ros/ros.h>
40 #include "sensor_msgs/PointCloud.h"
41 #include "sensor_msgs/LaserScan.h"
42 #include "laser_assembler/AssembleScans.h"
43 #include <boost/thread.hpp>
44 #include <boost/thread/condition.hpp>
45 
46 using namespace ros;
47 using namespace sensor_msgs;
48 using namespace std;
49 
50 static const string SERVICE_NAME = "assemble_scans";
51 
52 class TestAssembler : public testing::Test
53 {
54 public:
55 
57 
58  void SetUp()
59  {
60  ROS_INFO("Waiting for service [%s]", SERVICE_NAME.c_str());
62  ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str());
63  received_something_ = false;
64  got_cloud_ = false;
65  scan_sub_ = n_.subscribe("dummy_scan", 10, &TestAssembler::ScanCallback, (TestAssembler*)this);
66  }
67 
68  void ScanCallback(const LaserScanConstPtr& scan_msg)
69  {
70  boost::mutex::scoped_lock lock(mutex_);
71  if (!received_something_)
72  {
73  // Store the time of this first scan. Will be needed when we make the service call
74  start_time_ = scan_msg->header.stamp;
75  received_something_ = true;
76  }
77  else
78  {
79  // Make the call to get a point cloud
80  laser_assembler::AssembleScans assemble_scans;
81  assemble_scans.request.begin = start_time_;
82  assemble_scans.request.end = scan_msg->header.stamp;
83  EXPECT_TRUE(ros::service::call(SERVICE_NAME, assemble_scans));
84  if(assemble_scans.response.cloud.points.size() > 0)
85  {
86  ROS_INFO("Got a cloud with [%u] points. Saving the cloud", (uint32_t)(assemble_scans.response.cloud.points.size()));
87  cloud_ = assemble_scans.response.cloud;
88  got_cloud_ = true;
89  cloud_condition_.notify_all();
90  }
91  else
92  ROS_INFO("Got an empty cloud. Going to try again on the next scan");
93  }
94  }
95 
96 protected:
101  sensor_msgs::PointCloud cloud_;
102  boost::mutex mutex_;
103  boost::condition cloud_condition_;
104 };
105 
106 
108 {
109  ros::spin();
110 }
111 
112 // Check to make sure we can get a point cloud with at least 1 point in it
113 TEST_F(TestAssembler, non_zero_cloud_test)
114 {
115  // Wait until we get laser scans
116  boost::mutex::scoped_lock lock(mutex_);
117 
118  while(n_.ok() && !got_cloud_)
119  {
120  cloud_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000));
121  }
122 
123  EXPECT_LT((unsigned int) 0, cloud_.points.size());
124 
125  SUCCEED();
126 }
127 
128 int main(int argc, char** argv)
129 {
130  printf("******* Starting application *********\n");
131 
132  testing::InitGoogleTest(&argc, argv);
133  ros::init(argc, argv, "test_assembler_node");
134 
135  boost::thread spin_thread(spinThread);
136 
137  int result = RUN_ALL_TESTS();
138 
139  ros::shutdown();
140 
141  spin_thread.join();
142  return result;
143 }
TEST_F(TestAssembler, non_zero_cloud_test)
int main(int argc, char **argv)
void spinThread()
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool call(const std::string &service_name, MReq &req, MRes &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::condition cloud_condition_
sensor_msgs::PointCloud cloud_
ROSCPP_DECL void spin(Spinner &spinner)
boost::mutex mutex_
ros::Time start_time_
#define ROS_INFO(...)
ros::Subscriber scan_sub_
static const string SERVICE_NAME
int SUCCEED
ROSCPP_DECL void shutdown()
void ScanCallback(const LaserScanConstPtr &scan_msg)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Thu Jun 27 2019 19:47:56