kinematics_msgs: kinematics_msgs: utils.h Source 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 #ifndef KINEMATICS_MSGS_HELPER_FUNCTIONS_
00035 #define KINEMATICS_MSGS_HELPER_FUNCTIONS_
00036 
00037 #include <set>
00038 #include <vector>
00039 #include <string>
00040 
00041 namespace kinematics_msgs
00042 {
00054 void getCollisionLinks(const std::vector<std::string> &default_links, const std::vector<std::string> &collision_enable_links, const std::vector<std::string> &collision_disable_links, std::vector<std::string> &result)
00055 {
00056   if(collision_enable_links.empty() && collision_disable_links.empty())
00057     result = default_links;
00058   else if(!collision_enable_links.empty() && collision_disable_links.empty())
00059     result = collision_enable_links;
00060   else if(collision_enable_links.empty() && !collision_disable_links.empty())
00061   {
00062     std::set<std::string> tmp_set;
00063     for(unsigned int i=0; i < default_links.size(); i++)
00064       tmp_set.insert(default_links[i]);
00065     for(unsigned int i=0; i < collision_disable_links.size(); i++)
00066       tmp_set.erase(collision_disable_links[i]);
00067 
00068     for(std::set<std::string>::iterator set_iterator = tmp_set.begin(); set_iterator != tmp_set.end(); set_iterator++)
00069     {
00070        result.push_back(*set_iterator);
00071     }     
00072   }
00073   else 
00074   {
00075     std::set<std::string> tmp_set;
00076     for(unsigned int i=0; i < default_links.size(); i++)
00077       tmp_set.insert(default_links[i]);
00078     for(unsigned int i=0; i < collision_enable_links.size(); i++)
00079       tmp_set.insert(collision_enable_links[i]);
00080     for(unsigned int i=0; i < collision_disable_links.size(); i++)
00081       tmp_set.erase(collision_disable_links[i]);
00082 
00083     for(std::set<std::string>::iterator set_iterator = tmp_set.begin(); set_iterator != tmp_set.end(); set_iterator++)
00084     {
00085        result.push_back(*set_iterator);
00086     }
00087   }
00088 }
00089 
00090 }
00091 
00092 #endif