Fawkes API  Fawkes Development Version
utils.h
1 
2 /***************************************************************************
3  * utils.h - Fawkes tf utils
4  *
5  * Created: Fri Jun 1 14:07:00 2012
6  * Copyright 2012 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 /* This code is based on ROS tf with the following copyright and license:
24  *
25  * Copyright (c) 2008, Willow Garage, Inc.
26  * All rights reserved.
27  *
28  * Redistribution and use in source and binary forms, with or without
29  * modification, are permitted provided that the following conditions are met:
30  *
31  * * Redistributions of source code must retain the above copyright
32  * notice, this list of conditions and the following disclaimer.
33  * * Redistributions in binary form must reproduce the above copyright
34  * notice, this list of conditions and the following disclaimer in the
35  * documentation and/or other materials provided with the distribution.
36  * * Neither the name of the Willow Garage, Inc. nor the names of its
37  * contributors may be used to endorse or promote products derived from
38  * this software without specific prior written permission.
39  *
40  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
48  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50  * POSSIBILITY OF SUCH DAMAGE.
51  */
52 
53 #ifndef __LIBS_TF_UTILS_H_
54 #define __LIBS_TF_UTILS_H_
55 
56 #include <tf/types.h>
57 
58 namespace fawkes {
59  namespace tf {
60 #if 0 /* just to make Emacs auto-indent happy */
61  }
62 }
63 #endif
64 
65 /** Resolve transform name.
66  * @param prefix prefix to prepend to frame name
67  * @param frame_name frame name
68  * @return resolved frame name
69  */
70 inline std::string
71 resolve(const std::string& prefix, const std::string& frame_name)
72 {
73  if (frame_name.size() > 0) {
74  if (frame_name[0] == '/') {
75  return frame_name;
76  }
77  }
78  if (prefix.size() > 0) {
79  if (prefix[0] == '/') {
80  std::string composite = prefix;
81  composite.append("/");
82  composite.append(frame_name);
83  return composite;
84  } else {
85  std::string composite;
86  composite = "/";
87  composite.append(prefix);
88  composite.append("/");
89  composite.append(frame_name);
90  return composite;
91  }
92  } else {
93  std::string composite;
94  composite = "/";
95  composite.append(frame_name);
96  return composite;
97  }
98 }
99 
100 /** Create an ident pose in the given frame.
101  * An ident pose is with no translation and Quaternion (0,0,0,1), i.e. with
102  * all Euler angles zero.
103  * @param frame frame for which to get the ident transform
104  * @param t time for when to get the ident transform, defaults to (0,0) which
105  * means "latest possible time" for TF transforms.
106  * @return ident pose in given frame at given time
107  */
108 inline Stamped<Pose>
109 ident(std::string frame, Time t = Time(0,0))
110 {
111  return
112  tf::Stamped<tf::Pose>(tf::Transform(tf::Quaternion(0, 0, 0, 1),
113  tf::Vector3(0, 0, 0)),
114  t, frame);
115 }
116 
117 /** Resize a vector.
118  * @param v The reference vector that should be resized
119  * @param s The new length of the vector
120  * @return Vector with the new length 's'
121  */
122 inline Vector3
123 resize_vector(const Vector3 &v, double s)
124 {
125  return v.normalized() * s;
126 }
127 
128 
129 /** Check if frame ID starts with a slash (/).
130  * @param frame_id frame ID to check
131  * @return true if strings starts with slash, false otherwise
132  */
133 inline bool
134 starts_with_slash(const std::string& frame_id)
135 {
136  return (frame_id.size() > 0) && (frame_id[0] == '/');
137 }
138 
139 
140 /** Remove leading slash, if any.
141  * @param in frame_id to process
142  * @return returns @p in with leading slash removed, if any
143  */
144 inline std::string
145 strip_slash(const std::string& in)
146 {
147  if (starts_with_slash(in))
148  return in.substr(1);
149  else
150  return in;
151 }
152 
153 
154 } // end namespace tf
155 } // end namespace fawkes
156 
157 #endif
Fawkes library namespace.