shape_to_marker.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 
00035 #include <shape_tools/shape_to_marker.h>
00036 #include <sstream>
00037 #include <stdexcept>
00038 
00039 void shape_tools::constructMarkerFromShape(const shape_msgs::SolidPrimitive &shape_msg, visualization_msgs::Marker &mk)
00040 {  
00041   switch (shape_msg.type)
00042   {
00043   case shape_msgs::SolidPrimitive::SPHERE:
00044     if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::SPHERE_RADIUS)
00045       throw std::runtime_error("Insufficient dimensions in sphere definition");
00046     else
00047     {
00048       mk.type = visualization_msgs::Marker::SPHERE;
00049       mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
00050     }
00051     break;
00052   case shape_msgs::SolidPrimitive::BOX:
00053     if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_X ||
00054         shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Y ||
00055         shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Z)
00056       throw std::runtime_error("Insufficient dimensions in box definition");
00057     else
00058     {
00059       mk.type = visualization_msgs::Marker::CUBE;
00060       mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X];
00061       mk.scale.y = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
00062       mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
00063     }
00064     break;
00065   case shape_msgs::SolidPrimitive::CONE:
00066     if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_RADIUS ||
00067         shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_HEIGHT)
00068       throw std::runtime_error("Insufficient dimensions in cone definition");
00069     else
00070     {
00071       // there is no CONE marker, so this produces a cylinder marker as well 
00072       mk.type = visualization_msgs::Marker::CYLINDER;
00073       mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0;
00074       mk.scale.y = mk.scale.x;
00075       mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT];
00076     }
00077     break;
00078   case shape_msgs::SolidPrimitive::CYLINDER:
00079     if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_RADIUS ||
00080         shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)
00081       throw std::runtime_error("Insufficient dimensions in cylinder definition");
00082     else
00083     {
00084       mk.type = visualization_msgs::Marker::CYLINDER;
00085       mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0;
00086       mk.scale.y = mk.scale.x;
00087       mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
00088     }
00089     break;
00090   default:
00091     {
00092       std::stringstream ss;
00093       ss << shape_msg.type;
00094       throw std::runtime_error("Unknown shape type: " + ss.str());
00095     }
00096   }
00097 }
00098 
00099 void shape_tools::constructMarkerFromShape(const shape_msgs::Mesh &shape_msg, visualization_msgs::Marker &mk, bool use_mesh_triangle_list)
00100 {
00101   if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
00102     throw std::runtime_error("Mesh definition is empty");
00103   if (use_mesh_triangle_list)
00104   {
00105     mk.type = visualization_msgs::Marker::TRIANGLE_LIST;
00106     mk.scale.x = mk.scale.y = mk.scale.z = 1.0;
00107     for (std::size_t i = 0 ; i < shape_msg.triangles.size() ; ++i)
00108     {
00109       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
00110       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
00111       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
00112     }
00113   }
00114   else
00115   {
00116     mk.type = visualization_msgs::Marker::LINE_LIST;
00117     mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
00118     for (std::size_t i = 0 ; i < shape_msg.triangles.size() ; ++i)
00119     {
00120       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
00121       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
00122       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
00123       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
00124       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
00125       mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
00126     }
00127   }    
00128   
00129 }


shape_tools
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Oct 6 2014 07:37:21