35 #include <boost/lexical_cast.hpp> 39 int main(
int argc,
char** argv)
41 uint32_t num_levels = 10;
44 num_levels = boost::lexical_cast<uint32_t>(argv[1]);
54 bc.setTransform(t,
"me");
56 bc.setTransform(t,
"me");
58 for (uint32_t i = 1; i < num_levels/2; ++i)
60 for (uint32_t j = 1; j < 3; ++j)
62 std::stringstream parent_ss;
64 std::stringstream child_ss;
70 bc.setTransform(t,
"me");
79 bc.setTransform(t,
"me");
81 bc.setTransform(t,
"me");
83 for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
85 for (uint32_t j = 1; j < 3; ++j)
87 std::stringstream parent_ss;
89 std::stringstream child_ss;
95 bc.setTransform(t,
"me");
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());
106 const uint32_t count = 1000000;
107 ROS_INFO(
"Doing %d %d-level tests", count, num_levels);
112 for (uint32_t i = 0; i < count; ++i)
114 bc.lookupTransform(v_frame1, v_frame0,
ros::Time(0), 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);
126 for (uint32_t i = 0; i < count; ++i)
128 bc.lookupTransform(v_frame1, v_frame0,
ros::Time(1), 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);
140 for (uint32_t i = 0; i < count; ++i)
142 bc.lookupTransform(v_frame1, v_frame0,
ros::Time(1.5), 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);
154 for (uint32_t i = 0; i < count; ++i)
156 bc.lookupTransform(v_frame1, v_frame0,
ros::Time(2), 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);
168 for (uint32_t i = 0; i < count; ++i)
170 bc.canTransform(v_frame1, v_frame0,
ros::Time(0));
175 ROS_INFO(
"canTransform at Time(0) took %.3fs for an average of %.3f us", dur.
toSec(), dur.
toSec() * 1.0e6 / (double)count);
182 for (uint32_t i = 0; i < count; ++i)
184 bc.canTransform(v_frame1, v_frame0,
ros::Time(1));
189 ROS_INFO(
"canTransform at Time(1) took %.3fs for an average of %.3f us", dur.
toSec(), dur.
toSec() * 1.0e6 / (double)count);
196 for (uint32_t i = 0; i < count; ++i)
198 bc.canTransform(v_frame1, v_frame0,
ros::Time(1.5));
203 ROS_INFO(
"canTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.
toSec(), dur.
toSec() * 1.0e6 / (double)count);
210 for (uint32_t i = 0; i < count; ++i)
212 bc.canTransform(v_frame1, v_frame0,
ros::Time(2));
217 ROS_INFO(
"canTransform at Time(2) took %.3fs for an average of %.3f us", dur.
toSec(), dur.
toSec() * 1.0e6 / (double)count);
int main(int argc, char **argv)
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...