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 <tf/tf.h>
00031 
00032 #include <ros/time.h>
00033 #include <ros/assert.h>
00034 
00035 #include <boost/lexical_cast.hpp>
00036 
00037 using namespace tf;
00038 
00039 int main(int argc, char** argv)
00040 {
00041   uint32_t num_levels = 10;
00042   if (argc > 1)
00043   {
00044     num_levels = boost::lexical_cast<uint32_t>(argv[1]);
00045   }
00046 
00047   tf::Transformer bc;
00048   StampedTransform t;
00049   t.stamp_ = ros::Time(1);
00050   t.frame_id_ = "root";
00051   t.child_frame_id_ = "0";
00052   t.setIdentity();
00053   t.setOrigin(tf::Vector3(1,0,0));
00054   bc.setTransform(t, "me");
00055   t.stamp_ = ros::Time(2);
00056   bc.setTransform(t, "me");
00057 
00058   for (uint32_t i = 1; i < num_levels/2; ++i)
00059   {
00060     for (uint32_t j = 1; j < 3; ++j)
00061     {
00062       std::stringstream parent_ss;
00063       parent_ss << (i - 1);
00064       std::stringstream child_ss;
00065       child_ss << i;
00066 
00067       t.stamp_ = ros::Time(j);
00068       t.frame_id_ = parent_ss.str();
00069       t.child_frame_id_ = child_ss.str();
00070       bc.setTransform(t, "me");
00071     }
00072   }
00073 
00074   t.frame_id_ = "root";
00075   std::stringstream ss;
00076   ss << num_levels/2;
00077   t.stamp_ = ros::Time(1);
00078   t.child_frame_id_ = ss.str();
00079   bc.setTransform(t, "me");
00080   t.stamp_ = ros::Time(2);
00081   bc.setTransform(t, "me");
00082 
00083   for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
00084   {
00085     for (uint32_t j = 1; j < 3; ++j)
00086     {
00087       std::stringstream parent_ss;
00088       parent_ss << (i - 1);
00089       std::stringstream child_ss;
00090       child_ss << i;
00091 
00092       t.stamp_ = ros::Time(j);
00093       t.frame_id_ = parent_ss.str();
00094       t.child_frame_id_ = child_ss.str();
00095       bc.setTransform(t, "me");
00096     }
00097   }
00098 
00099   //ROS_INFO_STREAM(bc.allFramesAsYAML());
00100 
00101   std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
00102   std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
00103   ROS_INFO("%s to %s", v_frame0.c_str(), v_frame1.c_str());
00104   StampedTransform out_t;
00105 
00106   const uint32_t count = 1000000;
00107   ROS_INFO("Doing %d %d-level tests", count, num_levels);
00108 
00109 #if 01
00110   {
00111     ros::WallTime start = ros::WallTime::now();
00112     for (uint32_t i = 0; i < count; ++i)
00113     {
00114       bc.lookupTransform(v_frame1, v_frame0, ros::Time(0), out_t);
00115     }
00116     ros::WallTime end = ros::WallTime::now();
00117     ros::WallDuration dur = end - start;
00118     //ROS_INFO_STREAM(out_t);
00119     ROS_INFO("lookupTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00120   }
00121 #endif
00122 
00123 #if 01
00124   {
00125     ros::WallTime start = ros::WallTime::now();
00126     for (uint32_t i = 0; i < count; ++i)
00127     {
00128       bc.lookupTransform(v_frame1, v_frame0, ros::Time(1), out_t);
00129     }
00130     ros::WallTime end = ros::WallTime::now();
00131     ros::WallDuration dur = end - start;
00132     //ROS_INFO_STREAM(out_t);
00133     ROS_INFO("lookupTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00134   }
00135 #endif
00136 
00137 #if 01
00138   {
00139     ros::WallTime start = ros::WallTime::now();
00140     for (uint32_t i = 0; i < count; ++i)
00141     {
00142       bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5), out_t);
00143     }
00144     ros::WallTime end = ros::WallTime::now();
00145     ros::WallDuration dur = end - start;
00146     //ROS_INFO_STREAM(out_t);
00147     ROS_INFO("lookupTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00148   }
00149 #endif
00150 
00151 #if 01
00152   {
00153     ros::WallTime start = ros::WallTime::now();
00154     for (uint32_t i = 0; i < count; ++i)
00155     {
00156       bc.lookupTransform(v_frame1, v_frame0, ros::Time(2), out_t);
00157     }
00158     ros::WallTime end = ros::WallTime::now();
00159     ros::WallDuration dur = end - start;
00160     //ROS_INFO_STREAM(out_t);
00161     ROS_INFO("lookupTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00162   }
00163 #endif
00164 
00165 #if 01
00166   {
00167     ros::WallTime start = ros::WallTime::now();
00168     for (uint32_t i = 0; i < count; ++i)
00169     {
00170       bc.canTransform(v_frame1, v_frame0, ros::Time(0));
00171     }
00172     ros::WallTime end = ros::WallTime::now();
00173     ros::WallDuration dur = end - start;
00174     //ROS_INFO_STREAM(out_t);
00175     ROS_INFO("canTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00176   }
00177 #endif
00178 
00179 #if 01
00180   {
00181     ros::WallTime start = ros::WallTime::now();
00182     for (uint32_t i = 0; i < count; ++i)
00183     {
00184       bc.canTransform(v_frame1, v_frame0, ros::Time(1));
00185     }
00186     ros::WallTime end = ros::WallTime::now();
00187     ros::WallDuration dur = end - start;
00188     //ROS_INFO_STREAM(out_t);
00189     ROS_INFO("canTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00190   }
00191 #endif
00192 
00193 #if 01
00194   {
00195     ros::WallTime start = ros::WallTime::now();
00196     for (uint32_t i = 0; i < count; ++i)
00197     {
00198       bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
00199     }
00200     ros::WallTime end = ros::WallTime::now();
00201     ros::WallDuration dur = end - start;
00202     //ROS_INFO_STREAM(out_t);
00203     ROS_INFO("canTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00204   }
00205 #endif
00206 
00207 #if 01
00208   {
00209     ros::WallTime start = ros::WallTime::now();
00210     for (uint32_t i = 0; i < count; ++i)
00211     {
00212       bc.canTransform(v_frame1, v_frame0, ros::Time(2));
00213     }
00214     ros::WallTime end = ros::WallTime::now();
00215     ros::WallDuration dur = end - start;
00216     //ROS_INFO_STREAM(out_t);
00217     ROS_INFO("canTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count);
00218   }
00219 #endif
00220 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 27 2015 13:02:09