test_shape_extraction.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2014, Unbounded Robotics Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Unbounded Robotics, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Michael Ferguson
00031 
00032 #include <gtest/gtest.h>
00033 #include <simple_grasping/shape_extraction.h>
00034 
00035 TEST(test_shape_extraction, simple_cube)
00036 {
00037   // simple cube 4 corners of cube, plus one extra point
00038   pcl::PointCloud<pcl::PointXYZRGB> cloud;
00039   pcl::PointXYZRGB p;
00040   p.x = 0.2;
00041   p.y = 0.1;
00042   p.z = 0.1;
00043   cloud.push_back(p);
00044   p.x = 0.25;
00045   p.y = 0.1;
00046   cloud.push_back(p);
00047   p.x = 0.25;
00048   p.y = 0.15;
00049   cloud.push_back(p);
00050   p.x = 0.2;
00051   p.y = 0.15;
00052   cloud.push_back(p);
00053   p.x = 0.225;
00054   p.y = 0.14;
00055   cloud.push_back(p);
00056 
00057   pcl::PointCloud<pcl::PointXYZRGB> output;
00058 
00059   // create a plane at z = 0.05
00060   pcl::ModelCoefficients::Ptr plane(new pcl::ModelCoefficients);
00061   plane->values.resize(4);
00062   plane->values[0] = 0.0;
00063   plane->values[1] = 0.0;
00064   plane->values[2] = 1.0;
00065   plane->values[3] = -0.05;
00066 
00067   shape_msgs::SolidPrimitive shape;
00068   geometry_msgs::Pose pose;
00069 
00070   bool ret = simple_grasping::extractShape(cloud, plane, output, shape, pose);
00071   EXPECT_TRUE(ret);
00072 
00073   EXPECT_EQ(shape.BOX, shape.type);
00074   ASSERT_EQ(3, shape.dimensions.size());
00075   EXPECT_FLOAT_EQ(0.05, shape.dimensions[0]);
00076   EXPECT_FLOAT_EQ(0.05, shape.dimensions[1]);
00077   EXPECT_FLOAT_EQ(0.05, shape.dimensions[2]);
00078 
00079   EXPECT_FLOAT_EQ(0.225, pose.position.x);
00080   EXPECT_FLOAT_EQ(0.125, pose.position.y);
00081   EXPECT_FLOAT_EQ(0.075, pose.position.z);
00082 
00083   EXPECT_FLOAT_EQ(0.0, pose.orientation.x);
00084   EXPECT_FLOAT_EQ(0.0, pose.orientation.y);
00085   EXPECT_FLOAT_EQ(0.0, pose.orientation.z);
00086   EXPECT_FLOAT_EQ(1.0, pose.orientation.w);
00087 }
00088 
00089 // Run all the tests that were declared with TEST()
00090 int main(int argc, char **argv)
00091 {
00092   testing::InitGoogleTest(&argc, argv);
00093   return RUN_ALL_TESTS();
00094 }


simple_grasping
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:17:24