test_extract_indices.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
42 #include <ros/ros.h>
43 #include <gtest/gtest.h>
44 #include <sensor_msgs/PointCloud2.h>
45 #include <pcl_msgs/PointIndices.h>
47 
48 class ExtractIndicesTest: public testing::Test
49 {
50 protected:
51  virtual void SetUp()
52  {
53  // generate a pointcloud
55  // setup ROS
56  ros::NodeHandle n("~");
57  pub_cloud_ = n.advertise<sensor_msgs::PointCloud2>("output", 1);
58  pub_even_indices_ = n.advertise<pcl_msgs::PointIndices>("output/even_indices", 1);
59  pub_odd_indices_ = n.advertise<pcl_msgs::PointIndices>("output/odd_indices", 1);
60  sub_even_result_ = n.subscribe("input/even_result", 1, &ExtractIndicesTest::evenCallback, this);
61  sub_odd_result_ = n.subscribe("input/odd_result", 1, &ExtractIndicesTest::oddCallback, this);
62  sub_even_organized_result_ = n.subscribe("input/even_organized_result", 1,
64  sub_odd_organized_result_ = n.subscribe("input/odd_organized_result",
66 
67  // wait until
68  ros::Time start_time = ros::Time::now();
69  while (!isReady()) {
70  publishData();
71 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
72  /*
73  on melodic, it hugs after first test.
74  not sure why and if we move SetUp code to class function, then it will raise
75  terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >'
76  what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument
77  error on exit.
78  related ? -> https://github.com/ros/ros_comm/issues/838
79  */
80 #else
81  ros::spinOnce();
82 #endif
83  ros::Duration(0.5).sleep();
84  }
85  ros::Time end_time = ros::Time::now();
86  ROS_INFO("took %f sec to retrieve result", (end_time - start_time).toSec());
87  }
88 
89  virtual void generateTestData()
90  {
91  // cloud
92  original_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
93  for (size_t i = 0; i < 1000; i++) {
94  pcl::PointXYZ p;
95  p.x = i; p.y = 0; p.z = 0;
96  original_cloud_->points.push_back(p);
97  if (i % 2 == 0) {
98  even_indices_msg_.indices.push_back(i);
99  }
100  else {
101  odd_indices_msg_.indices.push_back(i);
102  }
103  }
105  }
106 
107  virtual bool isReady()
108  {
109  boost::mutex::scoped_lock lock(mutex_);
110  ROS_INFO("even_cloud_ %d, odd_cloud_ %d, even_organized_cloud_ %d, odd_organized_cloud_ %d",
113  }
114 
115  virtual void publishData()
116  {
118  original_cloud_msg_.header.stamp = now;
119  even_indices_msg_.header.stamp = now;
120  odd_indices_msg_.header.stamp = now;
124  }
125 
126  virtual void evenCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
127  {
128  boost::mutex::scoped_lock lock(mutex_);
129  even_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
130  pcl::fromROSMsg(*cloud_msg, *even_cloud_);
131  }
132 
133  virtual void oddCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
134  {
135  boost::mutex::scoped_lock lock(mutex_);
136  odd_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
137  pcl::fromROSMsg(*cloud_msg, *odd_cloud_);
138  }
139 
140  virtual void evenOrganizedCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
141  {
142  boost::mutex::scoped_lock lock(mutex_);
143  even_organized_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
145  }
146 
147  virtual void oddOrganizedCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
148  {
149  boost::mutex::scoped_lock lock(mutex_);
150  odd_organized_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
152  }
153 
158  pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud_;
159  pcl::PointCloud<pcl::PointXYZ>::Ptr even_cloud_, odd_cloud_;
160  pcl::PointCloud<pcl::PointXYZ>::Ptr even_organized_cloud_, odd_organized_cloud_;
161  sensor_msgs::PointCloud2 original_cloud_msg_;
162  pcl_msgs::PointIndices even_indices_msg_, odd_indices_msg_;
163 };
164 
165 TEST_F(ExtractIndicesTest, testEvenIndices)
166 {
167  EXPECT_EQ(even_cloud_->points.size(), even_indices_msg_.indices.size());
168  for (size_t i = 0; i < even_cloud_->points.size(); i++) {
169  EXPECT_FLOAT_EQ(even_cloud_->points[i].x, i * 2.0);
170  }
171 }
172 
173 TEST_F(ExtractIndicesTest, testOddIndices)
174 {
175  EXPECT_EQ(odd_cloud_->points.size(), odd_indices_msg_.indices.size());
176  for (size_t i = 0; i < odd_cloud_->points.size(); i++) {
177  EXPECT_FLOAT_EQ(odd_cloud_->points[i].x, i * 2.0 + 1.0);
178  }
179 }
180 
181 TEST_F(ExtractIndicesTest, testEvenOrganizedIndices)
182 {
183  EXPECT_EQ(even_organized_cloud_->points.size(), original_cloud_->points.size());
184  size_t non_nan_count = 0;
185  for (size_t i = 0; i < even_organized_cloud_->points.size(); i++) {
186  if (!std::isnan(even_organized_cloud_->points[i].x)) {
187  EXPECT_FLOAT_EQ(even_organized_cloud_->points[i].x, i);
188  non_nan_count++;
189  }
190  }
191  EXPECT_EQ(non_nan_count, even_indices_msg_.indices.size());
192 }
193 
194 TEST_F(ExtractIndicesTest, testOddOrganizedIndices)
195 {
196  EXPECT_EQ(odd_organized_cloud_->points.size(), original_cloud_->points.size());
197  size_t non_nan_count = 0;
198  for (size_t i = 0; i < odd_organized_cloud_->points.size(); i++) {
199  if (!std::isnan(odd_organized_cloud_->points[i].x)) {
200  EXPECT_FLOAT_EQ(odd_organized_cloud_->points[i].x, i);
201  non_nan_count++;
202  }
203  }
204  EXPECT_EQ(non_nan_count, odd_indices_msg_.indices.size());
205 }
206 
207 
208 int main(int argc, char** argv)
209 {
210  ros::init(argc, argv, "test_extract_indices");
211 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
213  spinner.start();
214 #endif
215  testing::InitGoogleTest(&argc, argv);
216  return RUN_ALL_TESTS();
217 }
virtual void oddCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
ros::Subscriber sub_even_result_
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
GLfloat n[6][3]
bool sleep() const
virtual void oddOrganizedCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
virtual void evenCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
ros::Subscriber sub_even_organized_result_
virtual void generateTestData()
ros::Publisher pub_odd_indices_
void spinner()
pcl::PointCloud< pcl::PointXYZ >::Ptr even_organized_cloud_
virtual void evenOrganizedCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
pcl_msgs::PointIndices odd_indices_msg_
#define EXPECT_FLOAT_EQ(a, b)
#define EXPECT_EQ(a, b)
pcl::PointCloud< pcl::PointXYZ >::Ptr original_cloud_
TEST_F(ExtractIndicesTest, testEvenIndices)
ros::Subscriber sub_odd_result_
#define ROS_INFO(...)
ros::Subscriber sub_odd_organized_result_
pcl::PointCloud< pcl::PointXYZ >::Ptr even_cloud_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
boost::mutex mutex
global mutex.
pcl::PointCloud< pcl::PointXYZ >::Ptr odd_organized_cloud_
p
static Time now()
sensor_msgs::PointCloud2 original_cloud_msg_
pcl::PointCloud< pcl::PointXYZ >::Ptr odd_cloud_
ros::Publisher pub_even_indices_
pcl_msgs::PointIndices even_indices_msg_
ROSCPP_DECL void spinOnce()
int main(int argc, char **argv)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47