cob_arm_navigation: move_arm_simple_pose

00001 
00058 #include <ros/ros.h>
00059 #include <actionlib/client/simple_action_client.h>
00060 
00061 #include <move_arm_msgs/MoveArmAction.h>
00062 #include <move_arm_msgs/utils.h>
00063 
00064 int main(int argc, char **argv){
00065   ros::init (argc, argv, "move_arm_pose_goal_test");
00066   ros::NodeHandle nh;
00067   actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_arm",true);
00068   move_arm.waitForServer();
00069   ROS_INFO("Connected to server");
00070   move_arm_msgs::MoveArmGoal goalA;
00071 
00072   goalA.motion_plan_request.group_name = "arm";
00073   goalA.motion_plan_request.num_planning_attempts = 1;
00074   goalA.motion_plan_request.planner_id = std::string("");
00075   goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
00076   goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00077   
00078   motion_planning_msgs::SimplePoseConstraint desired_pose;
00079   desired_pose.header.frame_id = "base_footprint";//"torso_lift_link";
00080   desired_pose.link_name = "arm_7_link";//"r_wrist_roll_link";
00081   
00082   
00083   //pos: grasp
00084 /*  desired_pose.pose.position.x =-0.615;
00085   desired_pose.pose.position.y = 0.064;
00086   desired_pose.pose.position.z = 0.800;
00087   desired_pose.pose.orientation.x = 0.224;
00088   desired_pose.pose.orientation.y =-0.714;
00089   desired_pose.pose.orientation.z = 0.277;
00090   desired_pose.pose.orientation.w = 0.603; */
00091   
00092   //pos: home
00093 /*  desired_pose.pose.position.x =-0.213;
00094   desired_pose.pose.position.y =-0.790;
00095   desired_pose.pose.position.z = 1.608;
00096   desired_pose.pose.orientation.x =-0.111;
00097   desired_pose.pose.orientation.y =-0.368;
00098   desired_pose.pose.orientation.z = 0.923;
00099   desired_pose.pose.orientation.w =-0.017; */
00100   
00101   //pos: wavein
00102   desired_pose.pose.position.x =-0.180;
00103   desired_pose.pose.position.y =-0.180;
00104   desired_pose.pose.position.z = 1.808;
00105   desired_pose.pose.orientation.x =-0.212;
00106   desired_pose.pose.orientation.y =-0.296;
00107   desired_pose.pose.orientation.z =-0.708;
00108   desired_pose.pose.orientation.w = 0.605;
00109   
00110 
00111   desired_pose.absolute_position_tolerance.x = 0.01;
00112   desired_pose.absolute_position_tolerance.y = 0.01;
00113   desired_pose.absolute_position_tolerance.z = 0.01;
00114 
00115   desired_pose.absolute_roll_tolerance = 0.01;
00116   desired_pose.absolute_pitch_tolerance = 0.01;
00117   desired_pose.absolute_yaw_tolerance = 0.01;
00118   
00119   move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
00120 
00121   if (nh.ok())
00122   {
00123     bool finished_within_time = false;
00124     move_arm.sendGoal(goalA);
00125     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00126     if (!finished_within_time)
00127     {
00128       move_arm.cancelGoal();
00129       ROS_INFO("Timed out achieving goal A");
00130     }
00131     else
00132     {
00133       actionlib::SimpleClientGoalState state = move_arm.getState();
00134       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00135       if(success)
00136         ROS_INFO("Action finished: %s",state.toString().c_str());
00137       else
00138         ROS_INFO("Action failed: %s",state.toString().c_str());
00139     }
00140   }
00141   ros::shutdown();
00142 }
00143
move_arm_simple_pose_goal.cpp
