00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00042 #include <ros/ros.h>
00043 #include <gtest/gtest.h>
00044 #include <sensor_msgs/PointCloud2.h>
00045 #include <pcl_msgs/PointIndices.h>
00046 #include <pcl_conversions/pcl_conversions.h>
00047
00048 class ExtractIndicesTest: public testing::Test
00049 {
00050 protected:
00051 virtual void SetUp()
00052 {
00053
00054 generateTestData();
00055
00056 ros::NodeHandle n("~");
00057 pub_cloud_ = n.advertise<sensor_msgs::PointCloud2>("output", 1);
00058 pub_even_indices_ = n.advertise<pcl_msgs::PointIndices>("output/even_indices", 1);
00059 pub_odd_indices_ = n.advertise<pcl_msgs::PointIndices>("output/odd_indices", 1);
00060 sub_even_result_ = n.subscribe("input/even_result", 1, &ExtractIndicesTest::evenCallback, this);
00061 sub_odd_result_ = n.subscribe("input/odd_result", 1, &ExtractIndicesTest::oddCallback, this);
00062 sub_even_organized_result_ = n.subscribe("input/even_organized_result", 1,
00063 &ExtractIndicesTest::evenOrganizedCallback, this);
00064 sub_odd_organized_result_ = n.subscribe("input/odd_organized_result",
00065 1, &ExtractIndicesTest::oddOrganizedCallback, this);
00066
00067
00068 ros::Time start_time = ros::Time::now();
00069 while (!isReady()) {
00070 publishData();
00071 ros::spinOnce();
00072 ros::Duration(0.5).sleep();
00073 }
00074 ros::Time end_time = ros::Time::now();
00075 ROS_INFO("took %f sec to retrieve result", (end_time - start_time).toSec());
00076 }
00077
00078 virtual void generateTestData()
00079 {
00080
00081 original_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00082 for (size_t i = 0; i < 1000; i++) {
00083 pcl::PointXYZ p;
00084 p.x = i; p.y = 0; p.z = 0;
00085 original_cloud_->points.push_back(p);
00086 if (i % 2 == 0) {
00087 even_indices_msg_.indices.push_back(i);
00088 }
00089 else {
00090 odd_indices_msg_.indices.push_back(i);
00091 }
00092 }
00093 pcl::toROSMsg(*original_cloud_, original_cloud_msg_);
00094 }
00095
00096 virtual bool isReady()
00097 {
00098 boost::mutex::scoped_lock lock(mutex_);
00099 return (even_cloud_ && odd_cloud_ && even_organized_cloud_ && odd_organized_cloud_);
00100 }
00101
00102 virtual void publishData()
00103 {
00104 ros::Time now = ros::Time::now();
00105 original_cloud_msg_.header.stamp = now;
00106 even_indices_msg_.header.stamp = now;
00107 odd_indices_msg_.header.stamp = now;
00108 pub_cloud_.publish(original_cloud_msg_);
00109 pub_even_indices_.publish(even_indices_msg_);
00110 pub_odd_indices_.publish(odd_indices_msg_);
00111 }
00112
00113 virtual void evenCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00114 {
00115 boost::mutex::scoped_lock lock(mutex_);
00116 even_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00117 pcl::fromROSMsg(*cloud_msg, *even_cloud_);
00118 }
00119
00120 virtual void oddCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00121 {
00122 boost::mutex::scoped_lock lock(mutex_);
00123 odd_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00124 pcl::fromROSMsg(*cloud_msg, *odd_cloud_);
00125 }
00126
00127 virtual void evenOrganizedCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00128 {
00129 boost::mutex::scoped_lock lock(mutex_);
00130 even_organized_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00131 pcl::fromROSMsg(*cloud_msg, *even_organized_cloud_);
00132 }
00133
00134 virtual void oddOrganizedCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00135 {
00136 boost::mutex::scoped_lock lock(mutex_);
00137 odd_organized_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00138 pcl::fromROSMsg(*cloud_msg, *odd_organized_cloud_);
00139 }
00140
00141 boost::mutex mutex_;
00142 ros::Subscriber sub_even_result_, sub_odd_result_;
00143 ros::Subscriber sub_even_organized_result_, sub_odd_organized_result_;
00144 ros::Publisher pub_cloud_, pub_even_indices_, pub_odd_indices_;
00145 pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud_;
00146 pcl::PointCloud<pcl::PointXYZ>::Ptr even_cloud_, odd_cloud_;
00147 pcl::PointCloud<pcl::PointXYZ>::Ptr even_organized_cloud_, odd_organized_cloud_;
00148 sensor_msgs::PointCloud2 original_cloud_msg_;
00149 pcl_msgs::PointIndices even_indices_msg_, odd_indices_msg_;
00150 };
00151
00152 TEST_F(ExtractIndicesTest, testEvenIndices)
00153 {
00154 EXPECT_EQ(even_cloud_->points.size(), even_indices_msg_.indices.size());
00155 for (size_t i = 0; i < even_cloud_->points.size(); i++) {
00156 EXPECT_FLOAT_EQ(even_cloud_->points[i].x, i * 2.0);
00157 }
00158 }
00159
00160 TEST_F(ExtractIndicesTest, testOddIndices)
00161 {
00162 EXPECT_EQ(odd_cloud_->points.size(), odd_indices_msg_.indices.size());
00163 for (size_t i = 0; i < odd_cloud_->points.size(); i++) {
00164 EXPECT_FLOAT_EQ(odd_cloud_->points[i].x, i * 2.0 + 1.0);
00165 }
00166 }
00167
00168 TEST_F(ExtractIndicesTest, testEvenOrganizedIndices)
00169 {
00170 EXPECT_EQ(even_organized_cloud_->points.size(), original_cloud_->points.size());
00171 size_t non_nan_count = 0;
00172 for (size_t i = 0; i < even_organized_cloud_->points.size(); i++) {
00173 if (!isnan(even_organized_cloud_->points[i].x)) {
00174 EXPECT_FLOAT_EQ(even_organized_cloud_->points[i].x, i);
00175 non_nan_count++;
00176 }
00177 }
00178 EXPECT_EQ(non_nan_count, even_indices_msg_.indices.size());
00179 }
00180
00181 TEST_F(ExtractIndicesTest, testOddOrganizedIndices)
00182 {
00183 EXPECT_EQ(odd_organized_cloud_->points.size(), original_cloud_->points.size());
00184 size_t non_nan_count = 0;
00185 for (size_t i = 0; i < odd_organized_cloud_->points.size(); i++) {
00186 if (!isnan(odd_organized_cloud_->points[i].x)) {
00187 EXPECT_FLOAT_EQ(odd_organized_cloud_->points[i].x, i);
00188 non_nan_count++;
00189 }
00190 }
00191 EXPECT_EQ(non_nan_count, odd_indices_msg_.indices.size());
00192 }
00193
00194
00195 int main(int argc, char** argv)
00196 {
00197 ros::init(argc, argv, "test_extract_indices");
00198 testing::InitGoogleTest(&argc, argv);
00199 return RUN_ALL_TESTS();
00200 }