environment_objects.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 "collision_space/environment_objects.h"
00038 #include <geometric_shapes/shape_operations.h>
00039 
00040 std::vector<std::string> collision_space::EnvironmentObjects::getNamespaces(void) const
00041 {
00042   std::vector<std::string> ns;
00043   for (std::map<std::string, NamespaceObjects>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
00044     ns.push_back(it->first);
00045   return ns;
00046 }
00047 
00048 const collision_space::EnvironmentObjects::NamespaceObjects& collision_space::EnvironmentObjects::getObjects(const std::string &ns) const
00049 {
00050   std::map<std::string, NamespaceObjects>::const_iterator it = objects_.find(ns);
00051   if (it == objects_.end())
00052     return empty_;
00053   else
00054     return it->second;
00055 }
00056 
00057 collision_space::EnvironmentObjects::NamespaceObjects& collision_space::EnvironmentObjects::getObjects(const std::string &ns)
00058 {
00059   return objects_[ns];
00060 }
00061 
00062 void collision_space::EnvironmentObjects::addObject(const std::string &ns, shapes::StaticShape *shape)
00063 {
00064   objects_[ns].static_shape.push_back(shape);
00065 }
00066 
00067 void collision_space::EnvironmentObjects::addObject(const std::string &ns, shapes::Shape *shape, const tf::Transform &pose)
00068 {
00069   objects_[ns].shape.push_back(shape);
00070   objects_[ns].shape_pose.push_back(pose);
00071 }
00072 
00073 bool collision_space::EnvironmentObjects::removeObject(const std::string &ns, const shapes::Shape *shape)
00074 {
00075   std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
00076   if (it != objects_.end())
00077   { 
00078     unsigned int n = it->second.shape.size();
00079     for (unsigned int i = 0 ; i < n ; ++i)
00080       if (it->second.shape[i] == shape)
00081       {
00082         it->second.shape.erase(it->second.shape.begin() + i);
00083         it->second.shape_pose.erase(it->second.shape_pose.begin() + i);
00084         return true;
00085       }
00086   }
00087   return false;
00088 }
00089 
00090 bool collision_space::EnvironmentObjects::removeObject(const std::string &ns, const shapes::StaticShape *shape)
00091 {
00092   std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
00093   if (it != objects_.end())
00094   { 
00095     unsigned int n = it->second.static_shape.size();
00096     for (unsigned int i = 0 ; i < n ; ++i)
00097       if (it->second.static_shape[i] == shape)
00098       {
00099         it->second.static_shape.erase(it->second.static_shape.begin() + i);
00100         return true;
00101       }
00102   }
00103   return false;
00104 }
00105 
00106 void collision_space::EnvironmentObjects::clearObjects(const std::string &ns)
00107 {
00108   std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
00109   if (it != objects_.end())
00110   {
00111     unsigned int n = it->second.static_shape.size();
00112     for (unsigned int i = 0 ; i < n ; ++i)
00113       delete it->second.static_shape[i];
00114     n = it->second.shape.size();
00115     for (unsigned int i = 0 ; i < n ; ++i)
00116       delete it->second.shape[i];
00117     objects_.erase(it);
00118   }
00119 }
00120 
00121 void collision_space::EnvironmentObjects::clearObjects(void)
00122 {
00123   std::vector<std::string> ns = getNamespaces();
00124   for (unsigned int i = 0 ; i < ns.size() ; ++i)
00125     clearObjects(ns[i]);
00126 }
00127 
00128 void collision_space::EnvironmentObjects::addObjectNamespace(const std::string ns)
00129 {
00130   if(objects_.find(ns) == objects_.end()) {
00131     objects_[ns] = NamespaceObjects();
00132   }
00133   //doesn't do anything if the object is already in objects_
00134 }
00135 
00136 collision_space::EnvironmentObjects* collision_space::EnvironmentObjects::clone(void) const
00137 {
00138   EnvironmentObjects *c = new EnvironmentObjects();
00139   for (std::map<std::string, NamespaceObjects>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
00140   {
00141     NamespaceObjects &ns = c->objects_[it->first];
00142     unsigned int n = it->second.static_shape.size();
00143     for (unsigned int i = 0 ; i < n ; ++i)
00144       ns.static_shape.push_back(shapes::cloneShape(it->second.static_shape[i]));
00145     n = it->second.shape.size();
00146     for (unsigned int i = 0 ; i < n ; ++i)
00147       ns.shape.push_back(shapes::cloneShape(it->second.shape[i]));
00148     ns.shape_pose = it->second.shape_pose;
00149   }
00150   return c;
00151 }


collision_space
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Thu Dec 12 2013 11:09:16