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
58namespace fawkes {
59namespace 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 */
66inline std::string
67resolve(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 */
104inline Stamped<Pose>
105ident(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 */
117inline Vector3
118resize_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 */
127inline bool
128starts_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 */
137inline std::string
138strip_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 library namespace.