00001 #include <ros/ros.h>
00002 #include <actionlib/client/simple_action_client.h>
00003 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00004 #include <planning_environment/monitors/joint_state_monitor.h>
00005 #include <gtest/gtest.h>
00006
00007 #include <sensor_msgs/JointState.h>
00008
00009 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> JointExecutorActionClient;
00010
00011 void spinThread()
00012 {
00013 ros::spin();
00014 }
00015
00016 TEST(OMPL, CollisionControllerTest)
00017 {
00018 JointExecutorActionClient *traj_action_client_ = new JointExecutorActionClient("collision_free_arm_trajectory_action_right_arm");
00019
00020 while(!traj_action_client_->waitForServer(ros::Duration(1.0))){
00021 ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
00022 if(!ros::ok()) {
00023 exit(0);
00024 }
00025 }
00026 planning_environment::JointStateMonitor joint_state_monitor;
00027
00028 pr2_controllers_msgs::JointTrajectoryGoal goal;
00029 goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00030 goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00031 goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00032 goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00033 goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00034 goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00035 goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00036
00037 goal.trajectory.points.resize(1);
00038 for(unsigned int i=0; i < 7; i++)
00039 goal.trajectory.points[0].positions.push_back(0.0);
00040 goal.trajectory.points[0].positions[0] = -1.57/2.0;
00041 goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
00042
00043 traj_action_client_->sendGoal(goal);
00044 ROS_INFO("Sent goal");
00045
00046 bool finished_within_time = false;
00047 finished_within_time = traj_action_client_->waitForResult(ros::Duration(100.0));
00048
00049 sensor_msgs::JointState joint_state = joint_state_monitor.getJointState(goal.trajectory.joint_names);
00050
00051 actionlib::SimpleClientGoalState state = traj_action_client_->getState();
00052 bool success = (state == actionlib::SimpleClientGoalState::ABORTED);
00053 EXPECT_TRUE(success);
00054
00055 EXPECT_TRUE(fabs(joint_state.position[0]) < 0.1);
00056
00057 }
00058
00059
00060 int main(int argc, char **argv){
00061 testing::InitGoogleTest(&argc, argv);
00062 ros::init(argc, argv, "test_controller");
00063 boost::thread spin_thread(&spinThread);
00064 return RUN_ALL_TESTS();
00065 }
