test_collision_space: test_collision_space: test_cs_point

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <cstdio>
00038 #include <cstdlib>
00039 #include <ros/ros.h>
00040 #include <algorithm>
00041 #include <visualization_msgs/Marker.h>
00042 #include <geometric_shapes/bodies.h>
00043 #include <geometric_shapes/shape_operations.h>
00044 
00045 const int TEST_TIMES  = 3;
00046 const int TEST_POINTS = 100000;
00047 
00048 class TestVM
00049 {
00050 public:
00051 
00052     TestVM(void) 
00053     {
00054         m_vmPub = m_nodeHandle.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
00055         m_id = 1;
00056     }
00057 
00058     virtual ~TestVM(void)
00059     {
00060     }
00061 
00062     void sendPoint(double x, double y, double z)
00063     {
00064         visualization_msgs::Marker mk;
00065 
00066         mk.header.stamp = m_tm;
00067 
00068         mk.header.frame_id = "base_link";
00069 
00070         mk.ns = "test_collision_space";
00071         mk.id = m_id++;
00072         mk.type = visualization_msgs::Marker::SPHERE;
00073         mk.action = visualization_msgs::Marker::ADD;
00074         mk.pose.position.x = x;
00075         mk.pose.position.y = y;
00076         mk.pose.position.z = z;
00077         mk.pose.orientation.w = 1.0;
00078 
00079         mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
00080 
00081         mk.color.a = 1.0;
00082         mk.color.r = 1.0;
00083         mk.color.g = 0.04;
00084         mk.color.b = 0.04;
00085 
00086         m_vmPub.publish(mk);
00087     }
00088 
00089     void testShape(bodies::Body *s)
00090     {
00091         for (int i = 0 ; i < TEST_POINTS ; ++i)
00092         {
00093             double x = uniform(5.0);
00094             double y = uniform(5.0);
00095             double z = uniform(5.0);
00096             if (!s->containsPoint(x, y, z))
00097                 continue;
00098             sendPoint(x, y, z);
00099         }
00100     }
00101 
00102     void setShapeTransformAndMarker(bodies::Body *s,
00103                                     visualization_msgs::Marker &mk)
00104     {
00105         btTransform t;
00106 
00107         double yaw   = uniform(M_PI);
00108         double pitch = uniform(M_PI);
00109         double roll  = uniform(M_PI);
00110 
00111         double x = uniform(3.0);
00112         double y = uniform(3.0);
00113         double z = uniform(3.0);
00114 
00115         t.setRotation(btQuaternion(yaw, pitch, roll));
00116         t.setOrigin(btVector3(btScalar(x), btScalar(y), btScalar(z)));
00117 
00118         s->setPose(t);
00119         s->setScale(1.0);
00120 
00121         mk.header.stamp = m_tm;
00122         mk.header.frame_id = "base_link";
00123         mk.ns = "test_collision_space";
00124         mk.id = m_id++;
00125 
00126         mk.action = visualization_msgs::Marker::ADD;
00127 
00128         mk.pose.position.x = x;
00129         mk.pose.position.y = y;
00130         mk.pose.position.z = z;
00131 
00132         btQuaternion orient(yaw, pitch, roll);
00133         mk.pose.orientation.x = orient.x();
00134         mk.pose.orientation.y = orient.y();
00135         mk.pose.orientation.z = orient.z();
00136         mk.pose.orientation.w = orient.w();
00137 
00138         mk.color.a = 0.55;
00139         mk.color.r = 0.0;
00140         mk.color.g = 0.4;
00141         mk.color.b = 1.0;
00142     }
00143 
00144     void testSphere(void)
00145     {
00146         shapes::Sphere shape(2.0);
00147         bodies::Body *s = new bodies::Sphere(&shape);
00148         printf("Sphere volume = %f\n", s->computeVolume());
00149         
00150         bodies::BoundingSphere sphere;
00151         s->computeBoundingSphere(sphere);
00152         
00153         printf("Bounding radius = %f\n", sphere.radius);
00154         for (int i = 0 ; i < TEST_TIMES ; ++i)
00155         {
00156             visualization_msgs::Marker mk;
00157             setShapeTransformAndMarker(s, mk);
00158 
00159             mk.type = visualization_msgs::Marker::SPHERE;
00160             mk.scale.x = shape.radius*2.0;
00161             mk.scale.y = shape.radius*2.0;
00162             mk.scale.z = shape.radius*2.0;
00163 
00164             m_vmPub.publish(mk);
00165 
00166             testShape(s);
00167         }
00168 
00169         delete s;
00170     }
00171 
00172     void testBox(void)
00173     {
00174         shapes::Box shape(2.0, 1.33, 1.5);
00175         bodies::Body *s = new bodies::Box(&shape);
00176         printf("Box volume = %f\n", s->computeVolume());
00177         
00178         bodies::BoundingSphere sphere;
00179         s->computeBoundingSphere(sphere);
00180         
00181         printf("Bounding radius = %f\n", sphere.radius);
00182 
00183         for (int i = 0 ; i < TEST_TIMES ; ++i)
00184         {
00185             visualization_msgs::Marker mk;
00186             setShapeTransformAndMarker(s, mk);
00187 
00188             mk.type = visualization_msgs::Marker::CUBE;
00189             mk.scale.x = shape.size[0]; // length
00190             mk.scale.y = shape.size[1]; // width
00191             mk.scale.z = shape.size[2]; // height
00192 
00193             m_vmPub.publish(mk);
00194 
00195             testShape(s);
00196         }
00197 
00198         delete s;
00199     }
00200 
00201     void testCylinder(void)
00202     {
00203         shapes::Cylinder shape(0.5, 2.5);
00204         bodies::Body *s = new bodies::Cylinder(&shape);
00205         printf("Cylinder volume = %f\n", s->computeVolume());
00206                 
00207         bodies::BoundingSphere sphere;
00208         s->computeBoundingSphere(sphere);
00209         
00210         printf("Bounding radius = %f\n", sphere.radius);
00211 
00212         for (int i = 0 ; i < TEST_TIMES ; ++i)
00213         {
00214             visualization_msgs::Marker mk;
00215             setShapeTransformAndMarker(s, mk);
00216 
00217             mk.type = visualization_msgs::Marker::CUBE;
00218             mk.scale.x = shape.radius * 2.0; // radius
00219             mk.scale.y = shape.radius * 2.0; // radius
00220             mk.scale.z = shape.length; //length
00221 
00222             m_vmPub.publish(mk);
00223 
00224             testShape(s);
00225         }
00226 
00227         delete s;
00228     }
00229 
00230     void testConvexMesh(void)
00231     {
00232         std::vector<btVector3> pts(12);
00233         btVector3 v1(0,0,1);
00234         btVector3 v2(1,0,-0.3);
00235         btVector3 v3(-0.5,0.8,-0.3);
00236         btVector3 v4(-0.5,-0.8,-0.3);
00237 
00238         pts[0] = v1;
00239         pts[1] = v2;
00240         pts[2] = v3;
00241 
00242         pts[3] = v1;
00243         pts[4] = v4;
00244         pts[5] = v3;
00245 
00246         pts[6] = v1;
00247         pts[7] = v4;
00248         pts[8] = v2;
00249 
00250         pts[9] = v4;
00251         pts[10] = v2;
00252         pts[11] = v3;
00253 
00254         //      shapes::Mesh *shape = shapes::create_mesh_from_binary_stl("base.stl");
00255         shapes::Mesh *shape = shapes::createMeshFromVertices(pts);
00256         bodies::Body *s = new bodies::ConvexMesh(shape);
00257         printf("Mesh volume = %f\n", s->computeVolume());
00258         
00259         bodies::BoundingSphere sphere;
00260         s->computeBoundingSphere(sphere);
00261         
00262         printf("Bounding radius = %f\n", sphere.radius);
00263 
00264         for (int i = 0 ; i < TEST_TIMES ; ++i)
00265         {
00266             visualization_msgs::Marker mk;
00267             setShapeTransformAndMarker(s, mk);
00268             testShape(s);
00269         }
00270 
00271         delete s;
00272         delete shape;
00273     }
00274 
00275     void run()
00276     {
00277         ros::Duration d;
00278         d.fromSec(2);
00279         d.sleep();
00280 
00281         m_tm = ros::Time::now();
00282 
00283         //      testSphere();
00284         //      testBox();
00285                 testCylinder();
00286         //      testConvexMesh();
00287 
00288         ROS_INFO("Idling...");
00289         ros::spin();
00290     }
00291 
00292 protected:
00293 
00294     // return a random number (uniform distribution)
00295     // between -magnitude and magnitude
00296     double uniform(double magnitude)
00297     {
00298         return (2.0 * drand48() - 1.0) * magnitude;
00299     }
00300 
00301     ros::NodeHandle           m_nodeHandle;
00302     ros::Publisher            m_vmPub;
00303     ros::Time                 m_tm;
00304     int                       m_id;
00305 
00306 };
00307 
00308 
00309 int main(int argc, char **argv)
00310 {
00311     ros::init(argc, argv, "TVM");
00312 
00313     TestVM tvm;
00314     tvm.run();
00315 
00316     return 0;
00317 }
test_cs_point_inclusion.cpp
