Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include <ros/ros.h>
00040 #include <tf/transform_listener.h>
00041 #include <boost/thread/thread.hpp>
00042 #include <urdf/model.h>
00043 #include <kdl_parser/kdl_parser.hpp>
00044 #include "robot_state_publisher/joint_state_listener.h"
00045
00046
00047 using namespace ros;
00048 using namespace tf;
00049 using namespace robot_state_publisher;
00050
00051
00052 int g_argc;
00053 char** g_argv;
00054
00055 #define EPS 0.01
00056
00057 class TestPublisher : public testing::Test
00058 {
00059 public:
00060 JointStateListener* publisher;
00061
00062 protected:
00064 TestPublisher()
00065 {}
00066
00068 ~TestPublisher()
00069 {}
00070 };
00071
00072
00073
00074
00075
00076 TEST_F(TestPublisher, test)
00077 {
00078 ROS_INFO("Creating tf listener");
00079 TransformListener tf;
00080
00081
00082
00083 ros::Duration(10.0).sleep();
00084
00085 ASSERT_TRUE(tf.canTransform("link1", "link2", Time()));
00086 ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
00087
00088 tf::StampedTransform t;
00089 tf.lookupTransform("link1", "link2",Time(), t );
00090 EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS);
00091 EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS);
00092 EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS);
00093
00094 SUCCEED();
00095 }
00096
00097
00098
00099
00100 int main(int argc, char** argv)
00101 {
00102 testing::InitGoogleTest(&argc, argv);
00103 ros::init(argc, argv, "test_robot_state_publisher");
00104 ros::NodeHandle node;
00105 boost::thread ros_thread(boost::bind(&ros::spin));
00106
00107 g_argc = argc;
00108 g_argv = argv;
00109 int res = RUN_ALL_TESTS();
00110 ros_thread.interrupt();
00111 ros_thread.join();
00112
00113 return res;
00114 }