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 ROS_INFO("Waiting for bag to complete");
00082 Duration(15.0).sleep();
00083
00084 ASSERT_TRUE(tf.canTransform("base_link", "torso_lift_link", Time()));
00085 ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time()));
00086 ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time()));
00087 ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "r_gripper_palm_link", Time()));
00088 ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "fl_caster_r_wheel_link", Time()));
00089 ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
00090
00091 tf::StampedTransform t;
00092 tf.lookupTransform("base_link", "r_gripper_palm_link",Time(), t );
00093 EXPECT_NEAR(t.getOrigin().x(), 0.769198, EPS);
00094 EXPECT_NEAR(t.getOrigin().y(), -0.188800, EPS);
00095 EXPECT_NEAR(t.getOrigin().z(), 0.764914, EPS);
00096
00097 tf.lookupTransform("l_gripper_palm_link", "r_gripper_palm_link",Time(), t );
00098 EXPECT_NEAR(t.getOrigin().x(), 0.000384222, EPS);
00099 EXPECT_NEAR(t.getOrigin().y(), -0.376021, EPS);
00100 EXPECT_NEAR(t.getOrigin().z(), -1.0858e-05, EPS);
00101
00102 SUCCEED();
00103 }
00104
00105
00106
00107
00108 int main(int argc, char** argv)
00109 {
00110 testing::InitGoogleTest(&argc, argv);
00111 ros::init(argc, argv, "test_robot_state_publisher");
00112 ros::NodeHandle node;
00113 boost::thread ros_thread(boost::bind(&ros::spin));
00114
00115 g_argc = argc;
00116 g_argv = argv;
00117 int res = RUN_ALL_TESTS();
00118 ros_thread.interrupt();
00119 ros_thread.join();
00120
00121 return res;
00122 }