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 #include <ros/ros.h>
00033 #include <gtest/gtest.h>
00034 #include <pr2_plugs_common/joint_space_move.h>
00035 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00036 #include <actionlib/server/simple_action_server.h>
00037 #include <boost/thread.hpp>
00038
00039 typedef actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> Server;
00040
00041 void execute(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr& goal, Server* as)
00042 {
00043 double joint_pos[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0};
00044 std::vector<double> pos (joint_pos, joint_pos + sizeof(joint_pos) / sizeof(double) );
00045 if(goal->trajectory.points[0].positions == pos)
00046 as->setSucceeded();
00047 else
00048 as->setAborted();
00049 }
00050
00051 void spinThread()
00052 {
00053 ros::spin();
00054 }
00055
00056
00057 TEST(JointSpaceMove, test_failure)
00058 {
00059
00060 EXPECT_TRUE(pr2_plugs_common::gotoJointPosition(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, true));
00061
00062 }
00063
00064 int main(int argc, char **argv){
00065 testing::InitGoogleTest(&argc, argv);
00066 ros::init(argc, argv, "trajectory_unwrap_test");
00067 ros::NodeHandle n;
00068 Server server(n, "r_arm_controller/joint_trajectory_action", boost::bind(&execute, _1, &server));
00069 boost::thread spin_thread(&spinThread);
00070 int log = RUN_ALL_TESTS();
00071 ros::shutdown();
00072 spin_thread.join();
00073 return log;
00074
00075 }