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("Publishing joint state to robot state publisher");
00082 ros::NodeHandle n;
00083 ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("joint_states", 100);
00084 sensor_msgs::JointState js_msg;
00085 js_msg.name.push_back("joint1");
00086 js_msg.position.push_back(M_PI);
00087 for (unsigned int i=0; i<100; i++){
00088 js_msg.header.stamp = ros::Time::now();
00089 js_pub.publish(js_msg);
00090 ros::Duration(0.1).sleep();
00091 }
00092
00093 ASSERT_TRUE(tf.canTransform("link1", "link2", Time()));
00094 ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
00095
00096 tf::StampedTransform t;
00097 tf.lookupTransform("link1", "link2",Time(), t );
00098 EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS);
00099 EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS);
00100 EXPECT_NEAR(t.getOrigin().z(), 0.0, 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 }