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 <tf2/buffer_core.h>
31 
32 #include <ros/time.h>
33 #include <console_bridge/console.h>
34 
35 #include <boost/lexical_cast.hpp>
36 
37 int main(int argc, char** argv)
38 {
39  uint32_t num_levels = 10;
40  if (argc > 1)
41  {
42  num_levels = boost::lexical_cast<uint32_t>(argv[1]);
43  }
44  double time_interval = 1.0;
45  if (argc > 2)
46  {
47  time_interval = boost::lexical_cast<double>(argv[2]);
48  }
49 
50  console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
51 
52  tf2::BufferCore bc;
53  geometry_msgs::TransformStamped t;
54  t.header.stamp = ros::Time(1);
55  t.header.frame_id = "root";
56  t.child_frame_id = "0";
57  t.transform.translation.x = 1;
58  t.transform.rotation.w = 1.0;
59  bc.setTransform(t, "me");
60  t.header.stamp = ros::Time(2);
61  bc.setTransform(t, "me");
62 
63  for (uint32_t i = 1; i < num_levels / 2; ++i)
64  {
65  for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
66  {
67  std::stringstream parent_ss;
68  parent_ss << (i - 1);
69  std::stringstream child_ss;
70  child_ss << i;
71 
72  t.header.stamp = ros::Time(j);
73  t.header.frame_id = parent_ss.str();
74  t.child_frame_id = child_ss.str();
75  bc.setTransform(t, "me");
76  }
77  }
78 
79  t.header.frame_id = "root";
80  std::stringstream ss;
81  ss << num_levels/2;
82  t.header.stamp = ros::Time(1);
83  t.child_frame_id = ss.str();
84  bc.setTransform(t, "me");
85  t.header.stamp = ros::Time(2);
86  bc.setTransform(t, "me");
87 
88  for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
89  {
90  for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
91  {
92  std::stringstream parent_ss;
93  parent_ss << (i - 1);
94  std::stringstream child_ss;
95  child_ss << i;
96 
97  t.header.stamp = ros::Time(j);
98  t.header.frame_id = parent_ss.str();
99  t.child_frame_id = child_ss.str();
100  bc.setTransform(t, "me");
101  }
102  }
103 
104  //logInfo_STREAM(bc.allFramesAsYAML());
105 
106  std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
107  std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
108  CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str());
109  geometry_msgs::TransformStamped out_t;
110 
111  const uint32_t count = 1000000;
112  CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);
113 
114 #if 01
115  {
117  for (uint32_t i = 0; i < count; ++i)
118  {
119  out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0));
120  }
122  ros::WallDuration dur = end - start;
123  //ROS_INFO_STREAM(out_t);
124  CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
125  }
126 #endif
127 
128 #if 01
129  {
131  for (uint32_t i = 0; i < count; ++i)
132  {
133  out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1));
134  }
136  ros::WallDuration dur = end - start;
137  //ROS_INFO_STREAM(out_t);
138  CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
139  }
140 #endif
141 
142 #if 01
143  {
145  for (uint32_t i = 0; i < count; ++i)
146  {
147  out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5));
148  }
150  ros::WallDuration dur = end - start;
151  //ROS_INFO_STREAM(out_t);
152  CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
153  }
154 #endif
155 
156 #if 01
157  {
159  for (uint32_t i = 0; i < count; ++i)
160  {
161  out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2));
162  }
164  ros::WallDuration dur = end - start;
165  //ROS_INFO_STREAM(out_t);
166  CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
167  }
168 #endif
169 
170 #if 01
171  {
173  for (uint32_t i = 0; i < count; ++i)
174  {
175  bc.canTransform(v_frame1, v_frame0, ros::Time(0));
176  }
178  ros::WallDuration dur = end - start;
179  //ROS_INFO_STREAM(out_t);
180  CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
181  }
182 #endif
183 
184 #if 01
185  {
187  for (uint32_t i = 0; i < count; ++i)
188  {
189  bc.canTransform(v_frame1, v_frame0, ros::Time(1));
190  }
192  ros::WallDuration dur = end - start;
193  //ROS_INFO_STREAM(out_t);
194  CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
195  }
196 #endif
197 
198 #if 01
199  {
201  for (uint32_t i = 0; i < count; ++i)
202  {
203  bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
204  }
206  ros::WallDuration dur = end - start;
207  //ROS_INFO_STREAM(out_t);
208  CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
209  }
210 #endif
211 
212 #if 01
213  {
215  for (uint32_t i = 0; i < count; ++i)
216  {
217  bc.canTransform(v_frame1, v_frame0, ros::Time(2));
218  }
220  ros::WallDuration dur = end - start;
221  //ROS_INFO_STREAM(out_t);
222  CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
223  }
224 #endif
225 }
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:88
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
Get the transform between two frames by frame ID.
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
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.
static WallTime now()
int main(int argc, char **argv)
Definition: speed_test.cpp:37


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 27 2022 02:43:06