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