construct_object.cpp
Go to the documentation of this file.
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 "planning_environment/util/construct_object.h"
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <ros/console.h>
00040 
00041 shapes::Shape* planning_environment::constructObject(const arm_navigation_msgs::Shape &obj)
00042 {
00043     shapes::Shape *shape = NULL;
00044     if (obj.type == arm_navigation_msgs::Shape::SPHERE)
00045     {
00046         if (obj.dimensions.size() != 1)
00047             ROS_ERROR("Unexpected number of dimensions in sphere definition");
00048         else
00049             shape = new shapes::Sphere(obj.dimensions[0]);
00050     }
00051     else
00052     if (obj.type == arm_navigation_msgs::Shape::BOX)
00053     {
00054         if (obj.dimensions.size() != 3)
00055             ROS_ERROR("Unexpected number of dimensions in box definition");
00056         else
00057             shape = new shapes::Box(obj.dimensions[0], obj.dimensions[1], obj.dimensions[2]);
00058     }
00059     else
00060     if (obj.type == arm_navigation_msgs::Shape::CYLINDER)
00061     {
00062         if (obj.dimensions.size() != 2)
00063             ROS_ERROR("Unexpected number of dimensions in cylinder definition");
00064         else
00065             shape = new shapes::Cylinder(obj.dimensions[0], obj.dimensions[1]);
00066     }   
00067     else
00068     if (obj.type == arm_navigation_msgs::Shape::MESH)
00069     {
00070         if (obj.dimensions.size() != 0)
00071             ROS_ERROR("Unexpected number of dimensions in mesh definition");
00072         else
00073         {
00074             if (obj.triangles.size() % 3 != 0)
00075                 ROS_ERROR("Number of triangle indices is not divisible by 3");
00076             else
00077             {
00078                 if (obj.triangles.empty() || obj.vertices.empty())
00079                     ROS_ERROR("Mesh definition is empty");
00080                 else
00081                 {
00082                     std::vector<tf::Vector3>    vertices(obj.vertices.size());
00083                     std::vector<unsigned int> triangles(obj.triangles.size());
00084                     for (unsigned int i = 0 ; i < obj.vertices.size() ; ++i)
00085                         vertices[i].setValue(obj.vertices[i].x, obj.vertices[i].y, obj.vertices[i].z);
00086                     for (unsigned int i = 0 ; i < obj.triangles.size() ; ++i)
00087                         triangles[i] = obj.triangles[i];
00088                     shape = shapes::createMeshFromVertices(vertices, triangles);
00089                 }
00090             }
00091         }
00092     }
00093     
00094     if (shape == NULL)
00095         ROS_ERROR("Unable to construct shape corresponding to object of type %d", (int)obj.type);
00096     
00097     return shape;
00098 }
00099 
00100 
00101 bool planning_environment::constructObjectMsg(const shapes::Shape* shape, arm_navigation_msgs::Shape &obj, double padding)
00102 {
00103   obj.dimensions.clear();
00104   obj.vertices.clear();
00105   obj.triangles.clear();
00106   if (shape->type == shapes::SPHERE)
00107   {
00108     obj.type = arm_navigation_msgs::Shape::SPHERE;
00109     obj.dimensions.push_back(static_cast<const shapes::Sphere*>(shape)->radius+padding);
00110   }
00111   else
00112     if (shape->type == shapes::BOX)
00113     {
00114       obj.type = arm_navigation_msgs::Shape::BOX;
00115       const double* sz = static_cast<const shapes::Box*>(shape)->size;  
00116       obj.dimensions.push_back(sz[0]+padding*2.0);
00117       obj.dimensions.push_back(sz[1]+padding*2.0);
00118       obj.dimensions.push_back(sz[2]+padding*2.0);
00119     }
00120     else
00121       if (shape->type == shapes::CYLINDER)
00122       { 
00123         obj.type = arm_navigation_msgs::Shape::CYLINDER;
00124         obj.dimensions.push_back(static_cast<const shapes::Cylinder*>(shape)->radius+padding);
00125         obj.dimensions.push_back(static_cast<const shapes::Cylinder*>(shape)->length+padding*2.0);
00126       }
00127       else
00128         if (shape->type == shapes::MESH)
00129         {
00130           obj.type = arm_navigation_msgs::Shape::MESH;
00131 
00132           const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
00133           const unsigned int t3 = mesh->triangleCount * 3;
00134 
00135           obj.vertices.resize(mesh->vertexCount);
00136           obj.triangles.resize(t3);
00137         
00138           double sx = 0.0, sy = 0.0, sz = 0.0;
00139           for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i)
00140           {
00141             unsigned int i3 = i * 3;
00142             obj.vertices[i].x = mesh->vertices[i3];
00143             obj.vertices[i].y = mesh->vertices[i3 + 1];
00144             obj.vertices[i].z = mesh->vertices[i3 + 2];
00145             sx += obj.vertices[i].x;
00146             sy += obj.vertices[i].y;
00147             sz += obj.vertices[i].z;
00148           }
00149           // the center of the mesh
00150           sx /= (double)mesh->vertexCount;
00151           sy /= (double)mesh->vertexCount;
00152           sz /= (double)mesh->vertexCount;
00153 
00154           // scale the mesh
00155           for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i)
00156           {
00157             // vector from center to the vertex
00158             double dx = obj.vertices[i].x - sx;
00159             double dy = obj.vertices[i].y - sy;
00160             double dz = obj.vertices[i].z - sz;
00161                 
00162             //double theta_xy = atan2(dy,dx);
00163             //double theta_xz = atan2(dz,dx);
00164 
00165             double ndx = ((dx > 0) ? dx+padding : dx-padding);
00166             double ndy = ((dy > 0) ? dy+padding : dy-padding);
00167             double ndz = ((dz > 0) ? dz+padding : dz-padding);
00168 
00169             obj.vertices[i].x = sx + ndx;
00170             obj.vertices[i].y = sy + ndy;
00171             obj.vertices[i].z = sz + ndz;
00172           }
00173 
00174           //for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i)
00175           //{
00176           //    obj.vertices[i].x = mesh->vertices[3 * i    ];
00177           //    obj.vertices[i].y = mesh->vertices[3 * i + 1];
00178           //    obj.vertices[i].z = mesh->vertices[3 * i + 2];
00179           //}
00180         
00181           for (unsigned int i = 0 ; i < t3  ; ++i)
00182             obj.triangles[i] = mesh->triangles[i];
00183         }
00184         else
00185         {
00186           ROS_ERROR("Unable to construct object message for shape of type %d", (int)shape->type);
00187           return false;
00188         }
00189     
00190   return true;
00191 }
00192 
00193 


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43