jsk_recognition_utils: test_tf_listener

Go to the documentation of this file.
00001 #include "jsk_recognition_utils/tf_listener_singleton.h"
00002 #include <ros/ros.h>
00003 #include <gtest/gtest.h>
00004 
00005 
00006 TEST(TfListenerSingleton, testLookupTransformWithDuration){
00007   boost::mutex mutex;
00008   tf::TransformListener* tf_listener;
00009 
00010   mutex.lock();
00011   tf_listener = new tf::TransformListener(ros::Duration(30.0));
00012   mutex.unlock();
00013 
00014   std::string from_frame("base");
00015   std::string to_frame("head");
00016   tf::StampedTransform transform;
00017   transform = jsk_recognition_utils::lookupTransformWithDuration(
00018     /*listener=*/tf_listener,
00019     /*to_frame=*/to_frame,
00020     /*from_frame=*/from_frame,
00021     /*time=*/ros::Time(),
00022     /*duration=*/ros::Duration(1.0));
00023   ASSERT_STREQ("base", transform.frame_id_.c_str());
00024   ASSERT_STREQ("head", transform.child_frame_id_.c_str());
00025   ASSERT_EQ(0, transform.getOrigin().getX());
00026   ASSERT_EQ(0, transform.getOrigin().getY());
00027   ASSERT_EQ(1, transform.getOrigin().getZ());
00028   ASSERT_EQ(0, transform.getRotation().getX());
00029   ASSERT_EQ(0, transform.getRotation().getY());
00030   ASSERT_EQ(0, transform.getRotation().getZ());
00031   ASSERT_EQ(1, transform.getRotation().getW());
00032 }
00033 
00034 int main(int argc, char **argv){
00035   ros::init(argc, argv, "simple_lookup_transform");
00036   testing::InitGoogleTest(&argc, argv);
00037   return RUN_ALL_TESTS();
00038 }