test_extract_indices.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
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     // generate a pointcloud
00054     generateTestData();
00055     // setup ROS
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     // wait until
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     // cloud
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 }


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50