00001 #include <trajectory_execution_monitor/trajectory_execution_monitor.h> 00002 #include <trajectory_execution_monitor/joint_state_recorder.h> 00003 #include <trajectory_execution_monitor/follow_joint_trajectory_controller_handler.h> 00004 #include <trajectory_execution_monitor/pr2_gripper_trajectory_controller_handler.h> 00005 00006 using namespace trajectory_execution_monitor; 00007 00008 bool trajectoryFinishedCallbackFunction(TrajectoryExecutionDataVector tedv) { 00009 ROS_INFO_STREAM("Trajectories " << tedv.size() << " ok " << (tedv.back().result_ == SUCCEEDED)); 00010 for(unsigned int i = 0; i < tedv.size(); i++) { 00011 ROS_INFO_STREAM("Recorded trajectory " << i << " has " << tedv[i].recorded_trajectory_.points.size() << " points" ); 00012 ROS_INFO_STREAM("Recorded trajectory " << i << " has time " << tedv[i].time_ << " seconds" ); 00013 ROS_INFO_STREAM("Recorded trajectory " << i << " has angular sum " << tedv[i].angular_distance_ << " radians" ); 00014 if(tedv[i].overshoot_trajectory_.points.size() > 0 ) 00015 { 00016 ROS_INFO_STREAM("Overshoot trajectory " << i << " has " << tedv[i].overshoot_trajectory_.points.size() << " points" ); 00017 ROS_INFO_STREAM("Overshoot trajectory " << i << " has time " << tedv[i].overshoot_time_ << " seconds" ); 00018 } 00019 } 00020 return true; 00021 } 00022 00023 int main(int argc, char** argv) { 00024 00025 ros::init(argc, argv, "trajectory_execution_monitor_test"); 00026 //figuring out whether robot_description has been remapped 00027 00028 ros::AsyncSpinner spinner(4); 00029 spinner.start(); 00030 00031 boost::shared_ptr<TrajectoryRecorder> tr(new JointStateTrajectoryRecorder("/joint_states")); 00032 boost::shared_ptr<TrajectoryControllerHandler> fjt( 00033 new FollowJointTrajectoryControllerHandler("right_arm", 00034 "/r_arm_controller/follow_joint_trajectory")); 00035 00036 boost::shared_ptr<TrajectoryControllerHandler> gripper( 00037 new Pr2GripperTrajectoryControllerHandler("r_end_effector", 00038 "/r_gripper_controller/gripper_action")); 00039 00040 TrajectoryExecutionMonitor tem; 00041 tem.addTrajectoryRecorder(tr); 00042 tem.addTrajectoryControllerHandler(fjt); 00043 tem.addTrajectoryControllerHandler(gripper); 00044 00045 std::vector<TrajectoryExecutionRequest> traj_reqs; 00046 00047 trajectory_msgs::JointTrajectory jt; 00048 jt.joint_names.push_back("r_shoulder_pan_joint"); 00049 jt.joint_names.push_back("r_shoulder_lift_joint"); 00050 jt.joint_names.push_back("r_upper_arm_roll_joint"); 00051 jt.joint_names.push_back("r_elbow_flex_joint"); 00052 jt.joint_names.push_back("r_forearm_roll_joint"); 00053 jt.joint_names.push_back("r_wrist_flex_joint"); 00054 jt.joint_names.push_back("r_wrist_roll_joint"); 00055 00056 jt.points.resize(100); 00057 00058 double start_angle = 0.0; 00059 double goal_angle = -.5; 00060 00061 if(argc > 2) { 00062 std::stringstream s(argv[1]); 00063 s >> start_angle; 00064 std::stringstream g(argv[2]); 00065 g >> goal_angle; 00066 } 00067 00068 ros::Duration r(3.0); 00069 00070 jt.header.stamp = ros::Time::now(); 00071 00072 for(unsigned int i=0; i < 100; i++) 00073 { 00074 jt.points[i].positions.push_back(start_angle+(i*1.0)*(goal_angle-start_angle)/100.0); 00075 jt.points[i].velocities.push_back(0.0); 00076 for(unsigned int j = 0; j < 6; j++) { 00077 jt.points[i].positions.push_back(0.0); 00078 jt.points[i].velocities.push_back(0.0); 00079 } 00080 jt.points[i].time_from_start = ros::Duration((i*1.0)*r.toSec()/(100.0)); 00081 } 00082 00083 TrajectoryExecutionRequest ter; 00084 ter.group_name_="right_arm"; 00085 ter.controller_name_ = "/r_arm_controller/follow_joint_trajectory"; 00086 ter.recorder_name_ = "/joint_states"; 00087 ter.trajectory_ = jt; 00088 ter.test_for_close_enough_ = true; 00089 ter.monitor_overshoot_ = true; 00090 ter.max_overshoot_time_ = ros::Duration(20); 00091 ter.min_overshoot_time_ = ros::Duration(0.5); 00092 ter.max_overshoot_velocity_epsilon_ = 0.01; 00093 ter.max_joint_distance_ = 0.001; 00094 ter.failure_time_factor_ = 1.5; 00095 traj_reqs.push_back(ter); 00096 00097 trajectory_msgs::JointTrajectory gt; 00098 gt.joint_names.push_back("r_gripper_l_finger_joint"); 00099 gt.joint_names.push_back("r_gripper_r_finger_joint"); 00100 gt.joint_names.push_back("r_gripper_r_finger_tip_joint"); 00101 gt.joint_names.push_back("r_gripper_l_finger_tip_joint"); 00102 00103 gt.points.resize(1); 00104 gt.points[0].positions.resize(4, .1); 00105 00106 TrajectoryExecutionRequest gter; 00107 gter.group_name_="r_end_effector"; 00108 gter.controller_name_ = "/r_gripper_controller/gripper_action"; 00109 gter.recorder_name_ = "/joint_states"; 00110 gter.trajectory_ = gt; 00111 gter.failure_ok_ = true; 00112 traj_reqs.push_back(gter); 00113 00114 jt.points.clear(); 00115 jt.points.resize(100); 00116 for(unsigned int i=0; i < 100; i++) 00117 { 00118 jt.points[i].positions.push_back(goal_angle+(i*1.0)*(start_angle-goal_angle)/100.0); 00119 jt.points[i].velocities.push_back(0.0); 00120 for(unsigned int j = 0; j < 6; j++) { 00121 jt.points[i].positions.push_back(0.0); 00122 jt.points[i].velocities.push_back(0.0); 00123 } 00124 jt.points[i].time_from_start = ros::Duration((i*1.0)*r.toSec()/(100.0)); 00125 } 00126 00127 ter.trajectory_ = jt; 00128 traj_reqs.push_back(ter); 00129 00130 gt.points[0].positions.clear(); 00131 gt.points[0].positions.resize(4, 0.0); 00132 gter.trajectory_ = gt; 00133 traj_reqs.push_back(gter); 00134 00135 boost::function<bool(TrajectoryExecutionDataVector)> f; 00136 f = trajectoryFinishedCallbackFunction; 00137 00138 tem.executeTrajectories(traj_reqs, f); 00139 00140 ros::waitForShutdown(); 00141 00142 return 0; 00143 }