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 
61 /** Resolve transform name.
62  * @param prefix prefix to prepend to frame name
63  * @param frame_name frame name
64  * @return resolved frame name
65  */
66 inline std::string
67 resolve(const std::string &prefix, const std::string &frame_name)
68 {
69  if (frame_name.size() > 0) {
70  if (frame_name[0] == '/') {
71  return frame_name;
72  }
73  }
74  if (prefix.size() > 0) {
75  if (prefix[0] == '/') {
76  std::string composite = prefix;
77  composite.append("/");
78  composite.append(frame_name);
79  return composite;
80  } else {
81  std::string composite;
82  composite = "/";
83  composite.append(prefix);
84  composite.append("/");
85  composite.append(frame_name);
86  return composite;
87  }
88  } else {
89  std::string composite;
90  composite = "/";
91  composite.append(frame_name);
92  return composite;
93  }
94 }
95 
96 /** Create an ident pose in the given frame.
97  * An ident pose is with no translation and Quaternion (0,0,0,1), i.e. with
98  * all Euler angles zero.
99  * @param frame frame for which to get the ident transform
100  * @param t time for when to get the ident transform, defaults to (0,0) which
101  * means "latest possible time" for TF transforms.
102  * @return ident pose in given frame at given time
103  */
104 inline Stamped<Pose>
105 ident(std::string frame, Time t = Time(0, 0))
106 {
107  return tf::Stamped<tf::Pose>(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
108  t,
109  frame);
110 }
111 
112 /** Resize a vector.
113  * @param v The reference vector that should be resized
114  * @param s The new length of the vector
115  * @return Vector with the new length 's'
116  */
117 inline Vector3
118 resize_vector(const Vector3 &v, double s)
119 {
120  return v.normalized() * s;
121 }
122 
123 /** Check if frame ID starts with a slash (/).
124  * @param frame_id frame ID to check
125  * @return true if strings starts with slash, false otherwise
126  */
127 inline bool
128 starts_with_slash(const std::string &frame_id)
129 {
130  return (frame_id.size() > 0) && (frame_id[0] == '/');
131 }
132 
133 /** Remove leading slash, if any.
134  * @param in frame_id to process
135  * @return returns @p in with leading slash removed, if any
136  */
137 inline std::string
138 strip_slash(const std::string &in)
139 {
140  if (starts_with_slash(in))
141  return in.substr(1);
142  else
143  return in;
144 }
145 
146 } // end namespace tf
147 } // end namespace fawkes
148 
149 #endif
fawkes
Fawkes library namespace.