00001 #include <ros/ros.h> 00002 #include <actionlib/client/simple_action_client.h> 00003 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00004 00005 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> JointExecutorActionClient; 00006 00007 void spinThread() 00008 { 00009 ros::spin(); 00010 } 00011 00012 int main(int argc, char** argv) 00013 { 00014 ros::init(argc, argv, "test_controller"); 00015 boost::thread spin_thread(&spinThread); 00016 JointExecutorActionClient *traj_action_client_ = new JointExecutorActionClient("collision_free_arm_trajectory_action_right_arm"); 00017 00018 while(!traj_action_client_->waitForServer(ros::Duration(1.0))){ 00019 ROS_INFO("Waiting for the joint_trajectory_action action server to come up"); 00020 if(!ros::ok()) { 00021 exit(0); 00022 } 00023 } 00024 00025 pr2_controllers_msgs::JointTrajectoryGoal goal; 00026 goal.trajectory.joint_names.push_back("r_shoulder_pan_joint"); 00027 goal.trajectory.joint_names.push_back("r_shoulder_lift_joint"); 00028 goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint"); 00029 goal.trajectory.joint_names.push_back("r_elbow_flex_joint"); 00030 goal.trajectory.joint_names.push_back("r_forearm_roll_joint"); 00031 goal.trajectory.joint_names.push_back("r_wrist_flex_joint"); 00032 goal.trajectory.joint_names.push_back("r_wrist_roll_joint"); 00033 00034 goal.trajectory.points.resize(1); 00035 for(unsigned int i=0; i < 7; i++) 00036 goal.trajectory.points[0].positions.push_back(0.0); 00037 goal.trajectory.points[0].positions[0] = -1.57/2.0; 00038 goal.trajectory.points[0].time_from_start = ros::Duration(0.0); 00039 00040 traj_action_client_->sendGoal(goal); 00041 ROS_INFO("Sent goal"); 00042 00043 while(!traj_action_client_->getState().isDone() && ros::ok()) 00044 { 00045 ros::Duration(0.1).sleep(); 00046 } 00047 return 0; 00048 }