pr2_arm_move_ik: pr2_arm_ik_test.cpp Source File

Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <actionlib/client/simple_action_client.h>
00003 #include <actionlib/client/terminal_state.h>
00004 #include <pr2_common_action_msgs/ArmMoveIKAction.h>
00005 
00006 int main (int argc, char **argv)
00007 {
00008   ros::init(argc, argv, "test_pr2_arm_ik");
00009 
00010   // create the action client                                                                                                            
00011   // true causes the client to spin it's own thread                                                                                      
00012   actionlib::SimpleActionClient<pr2_common_action_msgs::ArmMoveIKAction> ac("arm_ik", true);
00013 
00014   ROS_INFO("Waiting for action server to start.");
00015   // wait for the action server to start                                                                                                 
00016   ac.waitForServer(); //will wait for infinite time                                                                                      
00017 
00018   ROS_INFO("Action server started, sending goal.");
00019   // send a goal to the action                                                                                                           
00020   pr2_common_action_msgs::ArmMoveIKGoal goal;
00021   goal.pose.header.frame_id = "base_link";
00022   goal.pose.header.stamp = ros::Time::now();
00023   goal.pose.pose.orientation.x = 0.0;
00024   goal.pose.pose.orientation.y = 0.0;
00025   goal.pose.pose.orientation.z = 0.0;
00026   goal.pose.pose.orientation.w = 1.0;
00027   goal.pose.pose.position.x = 0.5;
00028   goal.pose.pose.position.y = -0.1;
00029   goal.pose.pose.position.z = 0.5;
00030   goal.ik_timeout = ros::Duration(5.0);
00031   goal.ik_seed.name.push_back("r_shoulder_pan_joint");
00032   goal.ik_seed.name.push_back("r_shoulder_lift_joint");
00033   goal.ik_seed.name.push_back("r_upper_arm_roll_joint");
00034   goal.ik_seed.name.push_back("r_elbow_flex_joint");
00035   goal.ik_seed.name.push_back("r_forearm_roll_joint");
00036   goal.ik_seed.name.push_back("r_wrist_flex_joint");
00037   goal.ik_seed.name.push_back("r_wrist_roll_joint");
00038   goal.ik_seed.position.push_back(-1.46);
00039   goal.ik_seed.position.push_back(1.09);
00040   goal.ik_seed.position.push_back(-6.58);
00041   goal.ik_seed.position.push_back(-1.73);
00042   goal.ik_seed.position.push_back(4.82);
00043   goal.ik_seed.position.push_back(-0.46);
00044   goal.ik_seed.position.push_back(-4.72);
00045   goal.move_duration= ros::Duration(1.0);
00046   ac.sendGoal(goal);
00047 
00048   //wait for the action to return                                                                                                        
00049   bool finished_before_timeout = ac.waitForResult(ros::Duration(40.0));
00050 
00051   if (finished_before_timeout)
00052   {
00053     actionlib::SimpleClientGoalState state = ac.getState();
00054     ROS_INFO("Action finished: %s",state.toString().c_str());
00055   }
00056   else
00057     ROS_INFO("Action did not finish before the time out.");
00058 
00059   //exit                                                                                                                                 
00060   return 0;
00061 }
00062