utils.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2012, Southwest Research Institute
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 are met:
00009  *
00010  *       * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *       * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *       * Neither the name of the Southwest Research Institute, nor the names
00016  *       of its contributors may be used to endorse or promote products derived
00017  *       from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #include "industrial_utils/utils.h"
00033 #include "ros/ros.h"
00034 #include <algorithm>
00035 
00036 namespace industrial_utils
00037 {
00038 
00039 bool isSimilar(std::vector<std::string> lhs, std::vector<std::string> rhs)
00040 {
00041   bool rtn = false;
00042 
00043   if (lhs.size() != rhs.size())
00044   {
00045     rtn = false;
00046   }
00047   else
00048   {
00049     std::sort(lhs.begin(), lhs.end());
00050     std::sort(rhs.begin(), rhs.end());
00051     rtn = isSame(lhs, rhs);
00052   }
00053 
00054   return rtn;
00055 }
00056 
00057 bool isSame(const std::vector<std::string> & lhs, const std::vector<std::string> & rhs)
00058 {
00059   bool rtn = false;
00060 
00061   if (lhs.size() != rhs.size())
00062   {
00063     rtn = false;
00064   }
00065   else
00066   {
00067     rtn = std::equal(lhs.begin(), lhs.end(), rhs.begin());
00068   }
00069   return rtn;
00070 }
00071 
00072 bool findChainJointNames(const boost::shared_ptr<const urdf::Link> &link, bool ignore_fixed,
00073                                  std::vector<std::string> &joint_names)
00074 {
00075   typedef std::vector<boost::shared_ptr<urdf::Joint> > joint_list;
00076   typedef std::vector<boost::shared_ptr<urdf::Link> > link_list;
00077   std::string found_joint, found_link;
00078 
00079   // check for joints directly connected to this link
00080   const joint_list &joints = link->child_joints;
00081   ROS_DEBUG("Found %lu child joints:", joints.size());
00082   for (joint_list::const_iterator it=joints.begin(); it!=joints.end(); ++it)
00083   {
00084     ROS_DEBUG_STREAM("  " << (*it)->name << ": type " <<  (*it)->type);
00085     if (ignore_fixed && (*it)->type == urdf::Joint::FIXED)
00086       continue;
00087 
00088     if (found_joint.empty())
00089     {
00090       found_joint = (*it)->name;
00091       joint_names.push_back(found_joint);
00092     }
00093     else
00094     {
00095       ROS_WARN_STREAM("Unable to find chain in URDF.  Branching joints: " << found_joint << " and " << (*it)->name);
00096       return false;  // branching tree (multiple valid child-joints)
00097     }
00098   }
00099 
00100   // check for joints connected to children of this link
00101   const link_list &links = link->child_links;
00102   std::vector<std::string> sub_joints;
00103   ROS_DEBUG("Found %lu child links:", links.size());
00104   for (link_list::const_iterator it=links.begin(); it!=links.end(); ++it)
00105   {
00106     ROS_DEBUG_STREAM("  " << (*it)->name);
00107     if (!findChainJointNames(*it, ignore_fixed, sub_joints))   // NOTE: recursive call
00108       return false;
00109 
00110     if (sub_joints.empty())
00111       continue;
00112 
00113     if (found_link.empty())
00114     {
00115       found_link = (*it)->name;
00116       joint_names.insert(joint_names.end(), sub_joints.begin(), sub_joints.end());  // append sub_joints
00117     }
00118     else
00119     {
00120       ROS_WARN_STREAM("Unable to find chain in URDF.  Branching links: " << found_link << " and " << (*it)->name);
00121       return false;  // branching tree (multiple valid child-joints)
00122     }
00123   }
00124 
00125   return true;
00126 }
00127 
00128 } //industrial_utils


industrial_utils
Author(s): Shaun M. Edwards
autogenerated on Sat Jun 8 2019 20:43:21