38 #include <gtest/gtest.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> 63 received_something_ =
false;
70 boost::mutex::scoped_lock lock(mutex_);
71 if (!received_something_)
74 start_time_ = scan_msg->header.stamp;
75 received_something_ =
true;
80 laser_assembler::AssembleScans assemble_scans;
81 assemble_scans.request.begin = start_time_;
82 assemble_scans.request.end = scan_msg->header.stamp;
84 if(assemble_scans.response.cloud.points.size() > 0)
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;
89 cloud_condition_.notify_all();
92 ROS_INFO(
"Got an empty cloud. Going to try again on the next scan");
116 boost::mutex::scoped_lock lock(mutex_);
118 while(n_.ok() && !got_cloud_)
120 cloud_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000));
123 EXPECT_LT((
unsigned int) 0, cloud_.points.size());
128 int main(
int argc,
char** argv)
130 printf(
"******* Starting application *********\n");
132 testing::InitGoogleTest(&argc, argv);
133 ros::init(argc, argv,
"test_assembler_node");
137 int result = RUN_ALL_TESTS();
TEST_F(TestAssembler, non_zero_cloud_test)
int main(int argc, char **argv)
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)
ros::Subscriber scan_sub_
static const string SERVICE_NAME
ROSCPP_DECL void shutdown()
void ScanCallback(const LaserScanConstPtr &scan_msg)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)