speed_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <tf/tf.h>
31 
32 #include <ros/time.h>
33 #include <ros/assert.h>
34 
35 #include <boost/lexical_cast.hpp>
36 
37 using namespace tf;
38 
39 int main(int argc, char** argv)
40 {
41  uint32_t num_levels = 10;
42  if (argc > 1)
43  {
44  num_levels = boost::lexical_cast<uint32_t>(argv[1]);
45  }
46 
47  tf::Transformer bc;
49  t.stamp_ = ros::Time(1);
50  t.frame_id_ = "root";
51  t.child_frame_id_ = "0";
52  t.setIdentity();
53  t.setOrigin(tf::Vector3(1,0,0));
54  bc.setTransform(t, "me");
55  t.stamp_ = ros::Time(2);
56  bc.setTransform(t, "me");
57 
58  for (uint32_t i = 1; i < num_levels/2; ++i)
59  {
60  for (uint32_t j = 1; j < 3; ++j)
61  {
62  std::stringstream parent_ss;
63  parent_ss << (i - 1);
64  std::stringstream child_ss;
65  child_ss << i;
66 
67  t.stamp_ = ros::Time(j);
68  t.frame_id_ = parent_ss.str();
69  t.child_frame_id_ = child_ss.str();
70  bc.setTransform(t, "me");
71  }
72  }
73 
74  t.frame_id_ = "root";
75  std::stringstream ss;
76  ss << num_levels/2;
77  t.stamp_ = ros::Time(1);
78  t.child_frame_id_ = ss.str();
79  bc.setTransform(t, "me");
80  t.stamp_ = ros::Time(2);
81  bc.setTransform(t, "me");
82 
83  for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
84  {
85  for (uint32_t j = 1; j < 3; ++j)
86  {
87  std::stringstream parent_ss;
88  parent_ss << (i - 1);
89  std::stringstream child_ss;
90  child_ss << i;
91 
92  t.stamp_ = ros::Time(j);
93  t.frame_id_ = parent_ss.str();
94  t.child_frame_id_ = child_ss.str();
95  bc.setTransform(t, "me");
96  }
97  }
98 
99  //ROS_INFO_STREAM(bc.allFramesAsYAML());
100 
101  std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
102  std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
103  ROS_INFO("%s to %s", v_frame0.c_str(), v_frame1.c_str());
104  StampedTransform out_t;
105 
106  const uint32_t count = 1000000;
107  ROS_INFO("Doing %d %d-level tests", count, num_levels);
108 
109 #if 01
110  {
112  for (uint32_t i = 0; i < count; ++i)
113  {
114  bc.lookupTransform(v_frame1, v_frame0, ros::Time(0), out_t);
115  }
117  ros::WallDuration dur = end - start;
118  //ROS_INFO_STREAM(out_t);
119  ROS_INFO("lookupTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
120  }
121 #endif
122 
123 #if 01
124  {
126  for (uint32_t i = 0; i < count; ++i)
127  {
128  bc.lookupTransform(v_frame1, v_frame0, ros::Time(1), out_t);
129  }
131  ros::WallDuration dur = end - start;
132  //ROS_INFO_STREAM(out_t);
133  ROS_INFO("lookupTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
134  }
135 #endif
136 
137 #if 01
138  {
140  for (uint32_t i = 0; i < count; ++i)
141  {
142  bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5), out_t);
143  }
145  ros::WallDuration dur = end - start;
146  //ROS_INFO_STREAM(out_t);
147  ROS_INFO("lookupTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
148  }
149 #endif
150 
151 #if 01
152  {
154  for (uint32_t i = 0; i < count; ++i)
155  {
156  bc.lookupTransform(v_frame1, v_frame0, ros::Time(2), out_t);
157  }
159  ros::WallDuration dur = end - start;
160  //ROS_INFO_STREAM(out_t);
161  ROS_INFO("lookupTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
162  }
163 #endif
164 
165 #if 01
166  {
168  for (uint32_t i = 0; i < count; ++i)
169  {
170  bc.canTransform(v_frame1, v_frame0, ros::Time(0));
171  }
173  ros::WallDuration dur = end - start;
174  //ROS_INFO_STREAM(out_t);
175  ROS_INFO("canTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
176  }
177 #endif
178 
179 #if 01
180  {
182  for (uint32_t i = 0; i < count; ++i)
183  {
184  bc.canTransform(v_frame1, v_frame0, ros::Time(1));
185  }
187  ros::WallDuration dur = end - start;
188  //ROS_INFO_STREAM(out_t);
189  ROS_INFO("canTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
190  }
191 #endif
192 
193 #if 01
194  {
196  for (uint32_t i = 0; i < count; ++i)
197  {
198  bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
199  }
201  ros::WallDuration dur = end - start;
202  //ROS_INFO_STREAM(out_t);
203  ROS_INFO("canTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
204  }
205 #endif
206 
207 #if 01
208  {
210  for (uint32_t i = 0; i < count; ++i)
211  {
212  bc.canTransform(v_frame1, v_frame0, ros::Time(2));
213  }
215  ros::WallDuration dur = end - start;
216  //ROS_INFO_STREAM(out_t);
217  ROS_INFO("canTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
218  }
219 #endif
220 }
tf::StampedTransform::stamp_
ros::Time stamp_
The timestamp associated with this transform.
Definition: transform_datatypes.h:84
tf::StampedTransform::child_frame_id_
std::string child_frame_id_
The frame_id of the coordinate frame this transform defines.
Definition: transform_datatypes.h:86
time.h
tf::Transformer::setTransform
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
Add transform information to the tf data structure.
Definition: tf.cpp:229
tf::Transformer::canTransform
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
Definition: tf.cpp:358
tf::Transformer
A Class which provides coordinate transforms between any two frames in a system.
Definition: tf.h:103
tf::StampedTransform
The Stamped Transform datatype used by tf.
Definition: transform_datatypes.h:81
tf::StampedTransform::frame_id_
std::string frame_id_
The frame_id of the coordinate frame in which this transform is defined.
Definition: transform_datatypes.h:85
tf.h
ros::WallTime::now
static WallTime now()
main
int main(int argc, char **argv)
Definition: speed_test.cpp:39
ros::WallTime
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Set the translational element.
Definition: Transform.h:148
start
ROSCPP_DECL void start()
ros::Time
tf::Transform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: Transform.h:170
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
Get the transform between two frames by frame ID.
Definition: tf.cpp:238
tf
Definition: exceptions.h:38
DurationBase< WallDuration >::toSec
double toSec() const
ros::WallDuration
assert.h
ROS_INFO
#define ROS_INFO(...)


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:07