33 #include <console_bridge/console.h> 35 #include <boost/lexical_cast.hpp> 37 int main(
int argc,
char** argv)
39 uint32_t num_levels = 10;
42 num_levels = boost::lexical_cast<uint32_t>(argv[1]);
44 double time_interval = 1.0;
47 time_interval = boost::lexical_cast<
double>(argv[2]);
50 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
53 geometry_msgs::TransformStamped t;
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;
63 for (uint32_t i = 1; i < num_levels / 2; ++i)
65 for (
double j = time_interval; j < 2.0 + time_interval; j += time_interval)
67 std::stringstream parent_ss;
69 std::stringstream child_ss;
73 t.header.frame_id = parent_ss.str();
74 t.child_frame_id = child_ss.str();
79 t.header.frame_id =
"root";
83 t.child_frame_id = ss.str();
88 for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
90 for (
double j = time_interval; j < 2.0 + time_interval; j += time_interval)
92 std::stringstream parent_ss;
94 std::stringstream child_ss;
98 t.header.frame_id = parent_ss.str();
99 t.child_frame_id = child_ss.str();
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;
111 const uint32_t count = 1000000;
112 CONSOLE_BRIDGE_logInform(
"Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);
117 for (uint32_t i = 0; i < count; ++i)
124 CONSOLE_BRIDGE_logInform(
"lookupTransform at Time(0) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
131 for (uint32_t i = 0; i < count; ++i)
138 CONSOLE_BRIDGE_logInform(
"lookupTransform at Time(1) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
145 for (uint32_t i = 0; i < count; ++i)
152 CONSOLE_BRIDGE_logInform(
"lookupTransform at Time(1.5) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
159 for (uint32_t i = 0; i < count; ++i)
166 CONSOLE_BRIDGE_logInform(
"lookupTransform at Time(2) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
173 for (uint32_t i = 0; i < count; ++i)
180 CONSOLE_BRIDGE_logInform(
"canTransform at Time(0) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
187 for (uint32_t i = 0; i < count; ++i)
194 CONSOLE_BRIDGE_logInform(
"canTransform at Time(1) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
201 for (uint32_t i = 0; i < count; ++i)
208 CONSOLE_BRIDGE_logInform(
"canTransform at Time(1.5) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
215 for (uint32_t i = 0; i < count; ++i)
222 CONSOLE_BRIDGE_logInform(
"canTransform at Time(2) took %f for an average of %.9f", dur.
toSec(), dur.
toSec() / (double)count);
A Class which provides coordinate transforms between any two frames in a system.
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.
int main(int argc, char **argv)