speed_test.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <tf2/buffer_core.h>
00031 
00032 #include <ros/time.h>
00033 #include <console_bridge/console.h>
00034 
00035 #include <boost/lexical_cast.hpp>
00036 
00037 int main(int argc, char** argv)
00038 {
00039   uint32_t num_levels = 10;
00040   if (argc > 1)
00041   {
00042     num_levels = boost::lexical_cast<uint32_t>(argv[1]);
00043   }
00044   double time_interval = 1.0;
00045   if (argc > 2)
00046   {
00047     time_interval = boost::lexical_cast<double>(argv[2]);
00048   }
00049 
00050   console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
00051 
00052   tf2::BufferCore bc;
00053   geometry_msgs::TransformStamped t;
00054   t.header.stamp = ros::Time(1);
00055   t.header.frame_id = "root";
00056   t.child_frame_id = "0";
00057   t.transform.translation.x = 1;
00058   t.transform.rotation.w = 1.0;
00059   bc.setTransform(t, "me");
00060   t.header.stamp = ros::Time(2);
00061   bc.setTransform(t, "me");
00062 
00063   for (uint32_t i = 1; i < num_levels / 2; ++i)
00064   {
00065     for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
00066     {
00067       std::stringstream parent_ss;
00068       parent_ss << (i - 1);
00069       std::stringstream child_ss;
00070       child_ss << i;
00071 
00072       t.header.stamp = ros::Time(j);
00073       t.header.frame_id = parent_ss.str();
00074       t.child_frame_id = child_ss.str();
00075       bc.setTransform(t, "me");
00076     }
00077   }
00078 
00079   t.header.frame_id = "root";
00080   std::stringstream ss;
00081   ss << num_levels/2;
00082   t.header.stamp = ros::Time(1);
00083   t.child_frame_id = ss.str();
00084   bc.setTransform(t, "me");
00085   t.header.stamp = ros::Time(2);
00086   bc.setTransform(t, "me");
00087 
00088   for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
00089   {
00090     for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
00091     {
00092       std::stringstream parent_ss;
00093       parent_ss << (i - 1);
00094       std::stringstream child_ss;
00095       child_ss << i;
00096 
00097       t.header.stamp = ros::Time(j);
00098       t.header.frame_id = parent_ss.str();
00099       t.child_frame_id = child_ss.str();
00100       bc.setTransform(t, "me");
00101     }
00102   }
00103 
00104   //logInfo_STREAM(bc.allFramesAsYAML());
00105 
00106   std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
00107   std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
00108   logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str());
00109   geometry_msgs::TransformStamped out_t;
00110 
00111   const uint32_t count = 1000000;
00112   logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);
00113 
00114 #if 01
00115   {
00116     ros::WallTime start = ros::WallTime::now();
00117     for (uint32_t i = 0; i < count; ++i)
00118     {
00119       out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0));
00120     }
00121     ros::WallTime end = ros::WallTime::now();
00122     ros::WallDuration dur = end - start;
00123     //ROS_INFO_STREAM(out_t);
00124     logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00125   }
00126 #endif
00127 
00128 #if 01
00129   {
00130     ros::WallTime start = ros::WallTime::now();
00131     for (uint32_t i = 0; i < count; ++i)
00132     {
00133       out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1));
00134     }
00135     ros::WallTime end = ros::WallTime::now();
00136     ros::WallDuration dur = end - start;
00137     //ROS_INFO_STREAM(out_t);
00138     logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00139   }
00140 #endif
00141 
00142 #if 01
00143   {
00144     ros::WallTime start = ros::WallTime::now();
00145     for (uint32_t i = 0; i < count; ++i)
00146     {
00147       out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5));
00148     }
00149     ros::WallTime end = ros::WallTime::now();
00150     ros::WallDuration dur = end - start;
00151     //ROS_INFO_STREAM(out_t);
00152     logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00153   }
00154 #endif
00155 
00156 #if 01
00157   {
00158     ros::WallTime start = ros::WallTime::now();
00159     for (uint32_t i = 0; i < count; ++i)
00160     {
00161       out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2));
00162     }
00163     ros::WallTime end = ros::WallTime::now();
00164     ros::WallDuration dur = end - start;
00165     //ROS_INFO_STREAM(out_t);
00166     logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00167   }
00168 #endif
00169 
00170 #if 01
00171   {
00172     ros::WallTime start = ros::WallTime::now();
00173     for (uint32_t i = 0; i < count; ++i)
00174     {
00175       bc.canTransform(v_frame1, v_frame0, ros::Time(0));
00176     }
00177     ros::WallTime end = ros::WallTime::now();
00178     ros::WallDuration dur = end - start;
00179     //ROS_INFO_STREAM(out_t);
00180     logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00181   }
00182 #endif
00183 
00184 #if 01
00185   {
00186     ros::WallTime start = ros::WallTime::now();
00187     for (uint32_t i = 0; i < count; ++i)
00188     {
00189       bc.canTransform(v_frame1, v_frame0, ros::Time(1));
00190     }
00191     ros::WallTime end = ros::WallTime::now();
00192     ros::WallDuration dur = end - start;
00193     //ROS_INFO_STREAM(out_t);
00194     logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00195   }
00196 #endif
00197 
00198 #if 01
00199   {
00200     ros::WallTime start = ros::WallTime::now();
00201     for (uint32_t i = 0; i < count; ++i)
00202     {
00203       bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
00204     }
00205     ros::WallTime end = ros::WallTime::now();
00206     ros::WallDuration dur = end - start;
00207     //ROS_INFO_STREAM(out_t);
00208     logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00209   }
00210 #endif
00211 
00212 #if 01
00213   {
00214     ros::WallTime start = ros::WallTime::now();
00215     for (uint32_t i = 0; i < count; ++i)
00216     {
00217       bc.canTransform(v_frame1, v_frame0, ros::Time(2));
00218     }
00219     ros::WallTime end = ros::WallTime::now();
00220     ros::WallDuration dur = end - start;
00221     //ROS_INFO_STREAM(out_t);
00222     logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
00223   }
00224 #endif
00225 }


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Tue Jul 10 2018 05:46:21