utils.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "industrial_utils/utils.h"
33 #include "ros/ros.h"
34 #include <algorithm>
35 
36 namespace industrial_utils
37 {
38 
39 bool isSimilar(std::vector<std::string> lhs, std::vector<std::string> rhs)
40 {
41  bool rtn = false;
42 
43  if (lhs.size() != rhs.size())
44  {
45  rtn = false;
46  }
47  else
48  {
49  std::sort(lhs.begin(), lhs.end());
50  std::sort(rhs.begin(), rhs.end());
51  rtn = isSame(lhs, rhs);
52  }
53 
54  return rtn;
55 }
56 
57 bool isSame(const std::vector<std::string> & lhs, const std::vector<std::string> & rhs)
58 {
59  bool rtn = false;
60 
61  if (lhs.size() != rhs.size())
62  {
63  rtn = false;
64  }
65  else
66  {
67  rtn = std::equal(lhs.begin(), lhs.end(), rhs.begin());
68  }
69  return rtn;
70 }
71 
72 bool findChainJointNames(const urdf::LinkConstSharedPtr &link, bool ignore_fixed,
73  std::vector<std::string> &joint_names)
74 {
75  typedef std::vector<urdf::JointSharedPtr > joint_list;
76  typedef std::vector<urdf::LinkSharedPtr > link_list;
77  std::string found_joint, found_link;
78 
79  // check for joints directly connected to this link
80  const joint_list &joints = link->child_joints;
81  ROS_DEBUG("Found %lu child joints:", joints.size());
82  for (joint_list::const_iterator it=joints.begin(); it!=joints.end(); ++it)
83  {
84  ROS_DEBUG_STREAM(" " << (*it)->name << ": type " << (*it)->type);
85  if (ignore_fixed && (*it)->type == urdf::Joint::FIXED)
86  continue;
87 
88  if (found_joint.empty())
89  {
90  found_joint = (*it)->name;
91  joint_names.push_back(found_joint);
92  }
93  else
94  {
95  ROS_WARN_STREAM("Unable to find chain in URDF. Branching joints: " << found_joint << " and " << (*it)->name);
96  return false; // branching tree (multiple valid child-joints)
97  }
98  }
99 
100  // check for joints connected to children of this link
101  const link_list &links = link->child_links;
102  std::vector<std::string> sub_joints;
103  ROS_DEBUG("Found %lu child links:", links.size());
104  for (link_list::const_iterator it=links.begin(); it!=links.end(); ++it)
105  {
106  ROS_DEBUG_STREAM(" " << (*it)->name);
107  if (!findChainJointNames(*it, ignore_fixed, sub_joints)) // NOTE: recursive call
108  return false;
109 
110  if (sub_joints.empty())
111  continue;
112 
113  if (found_link.empty())
114  {
115  found_link = (*it)->name;
116  joint_names.insert(joint_names.end(), sub_joints.begin(), sub_joints.end()); // append sub_joints
117  }
118  else
119  {
120  ROS_WARN_STREAM("Unable to find chain in URDF. Branching links: " << found_link << " and " << (*it)->name);
121  return false; // branching tree (multiple valid child-joints)
122  }
123  }
124 
125  return true;
126 }
127 
128 } //industrial_utils
bool findChainJointNames(const urdf::LinkConstSharedPtr &link, bool ignore_fixed, std::vector< std::string > &joint_names)
Definition: utils.cpp:72
bool isSame(const std::vector< std::string > &lhs, const std::vector< std::string > &rhs)
Checks to see if sets are the same(same members, same order) Wraps std::equal method.
Definition: utils.cpp:57
bool isSimilar(std::vector< std::string > lhs, std::vector< std::string > rhs)
Checks to see if sets are similar(same members, any order) NOTE: Vectors are passed by copy because t...
Definition: utils.cpp:39
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
#define ROS_DEBUG(...)


industrial_utils
Author(s): Shaun Edwards
autogenerated on Mon Feb 28 2022 22:34:34