32 #include <gtest/gtest.h> 39 std::srand(std::time(0));
42 void generate_rand_vectors(
double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
45 for ( uint64_t i = 0; i < runs ; i++ )
47 xvalues[i] = 1.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
48 yvalues[i] = 1.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
49 zvalues[i] = 1.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
63 EXPECT_STREQ(
"id", tl.
resolve(
"id").c_str());
65 n.
setParam(
"tf_prefix",
"a_tf_prefix");
70 EXPECT_STREQ(
"a_tf_prefix", prefix_str.c_str());
72 EXPECT_STREQ(
"a_tf_prefix/id",
tf::resolve(prefix_str,
"id").c_str());
74 EXPECT_STREQ(
"a_tf_prefix/id", tp.
resolve(
"id").c_str());
80 int main(
int argc,
char **argv){
81 testing::InitGoogleTest(&argc, argv);
82 ros::init(argc, argv,
"transform_listener_unittest");
83 return RUN_ALL_TESTS();
std::string getPrefixParam(ros::NodeHandle &nh)
Get the tf_prefix from the parameter server.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const