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 }